├── .gitignore ├── LICENSE ├── README.md ├── media ├── README.md └── thumbnail_ICUAS2025.jpeg ├── requirements.txt └── telloswarm_plus ├── PID.py ├── README.md ├── __init__.py ├── mocap.py ├── set-ap-mode.py ├── telloserver_EDU.py ├── telloserver_video.py └── test_api.py /.gitignore: -------------------------------------------------------------------------------- 1 | ### Python ### 2 | # Byte-compiled / optimized / DLL files 3 | __pycache__/ 4 | *.py[cod] 5 | *$py.class 6 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Mohammad Bahrami, 4 | Safe Autonomous Systems Lab, Stevens Institute of Technology 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TelloSwarm+ 2 | 3 | 4 | ### Updates 5 | 6 | - **August 2024** | *TelloSwarm+*: 7 | 8 | A multi-threaded API for formation control and video streaming from multiple [Tello EDU](https://www.ryzerobotics.com/tello-edu) drones over a wireless network (WLAN). 9 | 10 | See the [ICUAS_2025](https://github.com/SASLabStevens/TelloSwarm/tree/ICUAS_2025) branch for the code used in [Bahrami and Jafarnejadsani, IEEE ICUAS 2025](https://ieeexplore.ieee.org/abstract/document/11007832). 11 | 12 | - **June 2022** | *TelloSwarm*: 13 | 14 | An API for formation control of multiple *standard* [Tello](https://www.ryzerobotics.com/tello) drones over a wireless network (WLAN). 15 | 16 | See the [ICUAS_2022](https://github.com/SASLabStevens/TelloSwarm/tree/ICUAS_2022) branch for the code used in [Bahrami and Jafarnejadsani, IEEE ICUAS 2022](https://ieeexplore.ieee.org/abstract/document/9836208). 17 | --- 18 | 19 | If you find this code useful in your research, please consider citing our papers: 20 | 21 | ``` 22 | @inproceedings{bahrami2025multi, 23 | title={Multi-Robot Coordination with Adversarial Perception}, 24 | author={Bahrami, Rayan and Jafarnejadsani, Hamidreza}, 25 | booktitle={2025 International Conference on Unmanned Aircraft Systems (ICUAS)}, 26 | pages={370--377}, 27 | year={2025}, 28 | organization={IEEE} 29 | doi={10.1109/ICUAS65942.2025.11007832} 30 | } 31 | ``` 32 | 33 | ``` 34 | @INPROCEEDINGS{9836208, 35 | author={Bahrami, Rayan and Jafarnejadsani, Hamidreza}, 36 | booktitle={2022 International Conference on Unmanned Aircraft Systems (ICUAS)}, 37 | title={Detection of Stealthy Adversaries for Networked Unmanned Aerial Vehicles}, 38 | year={2022}, 39 | volume={}, 40 | number={}, 41 | pages={1111-1120}, 42 | doi={10.1109/ICUAS54217.2022.9836208}} 43 | ``` 44 | For details, check out the [PDF](https://arxiv.org/abs/2202.09661), the [video](https://youtu.be/lVT_muezKLU), and the following [instructions](#instructions). 45 | 46 |

47 | 48 | MRC 49 | 50 |

51 | 52 | # Instructions 53 | 54 | The code has been tested on Ubuntu 20.04 and ROS Noetic with Python 3.8+. 55 | 56 | ### Installation 57 | 58 | - **Install TelloSwarm+ with pip** 59 | ``` 60 | git clone https://github.com/SASLabStevens/TelloSwarm 61 | pip install -r requirements.txt 62 | ``` 63 | 64 | - **Install with ROS** (optional for motion capture integration) 65 | 66 | [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) and catkin tools are required. 67 | 68 | ``` 69 | # Create a catkin workspace named telloswarm_ws and set the current directory to the source (src) folder: 70 | mkdir -p ~/telloswarm_ws/src && cd ~/telloswarm_ws/src 71 | 72 | # Clone the vicon_bridge and TelloSwarm_plus repositories: 73 | git clone https://github.com/r-bahrami/vicon_bridge && git clone https://github.com/SASLabStevens/TelloSwarm 74 | 75 | # Build and source the setup file: 76 | cd ~/telloswarm_ws/ && catkin_make 77 | # 78 | source devel/setup.bash 79 | 80 | # install packages 81 | pip install -r requirements.txt 82 | ``` 83 | 84 | In the `vicon.launch` file located in `cd ~/telloswarm_ws/src/vicon_bridge/launch`, you may need to set the "datastream_hostport" parameter to the IP/hostname of your vicon's host PC and the "stream_mode" parameter to either of "ServerPush", "ClientPull", or "ClientPullPreFetch" modes. For details, consult [Vicon DataStream SDK Developer's Guide](https://docs.vicon.com/display/DSSDK111/DataStream+SDK+Documentation). 85 | 86 | Run the `vicon.launch` file and `mocap.py` to check if ROS and Python packages work: 87 | 88 | In the already-opened terminal run 89 | 90 | ``` 91 | roslaunch vicon_bridge vicon.launch 92 | ``` 93 | 94 | which will start streaming motion capture data using ROS. 95 | 96 | In another terminal, run `mocap.py` to access the ground truth pose of a drone defined in the VICON Tracker. When asked to enter the "OBJECT_Name", it is the name of a drone created in the VICON Tracker. 97 | ``` 98 | python3 ~/telloswarm_ws/src/TelloSwarm/telloswarm_plus/mocap.py 99 | ``` 100 | 101 | ### How to use 102 | 103 | For network configuration and and instructions on how to use this code, please refer to the [README](https://github.com/SASLabStevens/TelloSwarm/tree/main/telloswarm_plus) file and the sample code `test_api.py` inside the `telloswarm_plus` directory. 104 | -------------------------------------------------------------------------------- /media/README.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /media/thumbnail_ICUAS2025.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SASLabStevens/TelloSwarm/25af2e88289b9dd8d00b645bed8d0c200b7156b1/media/thumbnail_ICUAS2025.jpeg -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # ==== base ===== 2 | numpy>=1.18.5 # ,<=1.20 # the upperbound is needed for custom training yolo 3 | # matplotlib>=3.2.2 4 | opencv-python>=4.1.2 5 | # Pillow 6 | # scipy>=1.4.1 7 | # ==== for mocap ===== 8 | rospkg 9 | PyYAML>=5.3.1 10 | 11 | 12 | -------------------------------------------------------------------------------- /telloswarm_plus/PID.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Thu Aug 5 17:51:22 2021 5 | 6 | @author: Rayan Bahrami 7 | 8 | Safe Autonomous Systems Lab (SAS Lab) 9 | Stevens Institute of Technology 10 | 11 | """ 12 | # fmt: off 13 | import time 14 | 15 | 16 | class PID: 17 | """ 18 | PID controller with a lowpass filter on D term. 19 | Backward Euler method are used in approximations. 20 | 21 | """ 22 | def __init__(self, Kp: float, Kd: float, Ki: float, 23 | derivativeFilterFreq: int, 24 | # previousTime: None, 25 | minOutput: float, maxOutput: float, 26 | current_time = None): 27 | """ 28 | TODO: 29 | 30 | 31 | Parameters 32 | ---------- 33 | Kp : float 34 | DESCRIPTION. 35 | Kd : float 36 | DESCRIPTION. 37 | Ki : float 38 | DESCRIPTION. 39 | derivativeFilterFreq : int 40 | DESCRIPTION. 41 | previousTime : float 42 | DESCRIPTION. 43 | minOutput : TYPE, optional 44 | DESCRIPTION. The default is None. 45 | maxOutput : TYPE, optional 46 | DESCRIPTION. The default is None. 47 | current_time : TYPE, optional 48 | DESCRIPTION. The default is None. 49 | 50 | Returns 51 | ------- 52 | None. 53 | 54 | """ 55 | 56 | self.Kp = Kp 57 | self.Kd = Kd 58 | self.Ki = Ki 59 | self.F = derivativeFilterFreq 60 | # self.sample_time = sampleTime 61 | self.Ki = Ki 62 | self.minOutput = minOutput 63 | self.maxOutput = maxOutput 64 | self.previousError = 0.0 65 | self.previousDterm = 0.0 66 | self.previousIterm = 0.0 67 | # self.currentTime = current_time if current_time is not None else time.time() 68 | # self.previousTime = ros::Time::now() 69 | # self.previousTime = self.currentTime 70 | self.previousTime = current_time if current_time is not None else time.time() 71 | 72 | 73 | # self.reset(self) 74 | 75 | def reset(self, current_time = None): 76 | self.previousError = 0.0 77 | self.previousDterm = 0.0 78 | self.previousIterm = 0.0 79 | # self.previousTime = ros::Time::now() 80 | self.previousTime = current_time if current_time is not None else time.time() 81 | 82 | 83 | def update(self, feedback_value=None, target_value=None, tracking_error=None, current_time=None): 84 | """ 85 | 86 | 87 | Parameters 88 | ---------- 89 | feedback_value : TYPE 90 | DESCRIPTION. 91 | target_value : TYPE 92 | DESCRIPTION. 93 | tracking_error : TYPE, optional 94 | DESCRIPTION. The default is None. 95 | current_time : TYPE, optional 96 | DESCRIPTION. The default is None. 97 | 98 | Returns 99 | ------- 100 | TYPE 101 | DESCRIPTION. 102 | 103 | """ 104 | 105 | if tracking_error is None and feedback_value is None: 106 | raise Exception("ERROR! PID requires either tracking_error OR both feedback_value and target_value as input.") 107 | 108 | if tracking_error is None and target_value is None: 109 | raise Exception("ERROR! PID requires either tracking_error OR both feedback_value and target_value as input.") 110 | 111 | 112 | 113 | # TOO: add if {dt>0 OR Ts} for updating the D term 114 | # what if feedback_value, target_value are not given 115 | 116 | 117 | currentTime = current_time if current_time is not None else time.time() 118 | error = (target_value - feedback_value) if tracking_error is None else tracking_error 119 | dt = currentTime - self.previousTime 120 | 121 | Pterm = self.Kp * error 122 | Dterm = self.previousDterm/(1+dt*self.F) + ( 123 | self.Kd*self.F*(error - self.previousError))/(1+self.F*dt) 124 | Iterm = self.previousIterm + self.Ki * dt * error 125 | Iterm = max(self.minOutput*.4, min(self.maxOutput*.4, Iterm)) 126 | 127 | # self.currentTime = current_time if current_time is not None else time.time() 128 | self.previousError = error 129 | self.previousDterm = Dterm 130 | self.previousIterm = Iterm 131 | self.previousTime = currentTime 132 | output = Pterm + Dterm + Iterm 133 | return max(self.minOutput, min(self.maxOutput, output)) 134 | -------------------------------------------------------------------------------- /telloswarm_plus/README.md: -------------------------------------------------------------------------------- 1 | ## Multi-threaded Python API for Tello EDU 2 | 3 | 4 | - `telloserver_EDU.py` provides an API for swarming / formation control over 2.45 GHz WLAN 5 | - `telloserver_video.py` [optional] enables video streaming from multiple drones 6 | - `mocap.py` [optional] provides an API to get the drones' poses from a motion capture system (VICON) 7 | - `mocap.py` relies on `rospy` and [vicon_bridge](https://github.com/ethz-asl/vicon_bridge) 8 | 9 | ### Network Configuration 10 | 11 | - use `python set-ap-mode.py -s [SSID] -p [PASSWORD]` to put each drones on a router. Then, set a static IP address to each drone in the DHCP settings of the router. This step is done only once. 12 | 13 | - use the static `IP` address of each drone, the drone's `name` in the VICON Tracker, and a `port` for video streaming in a dictionary as follows: 14 | 15 | ```python 16 | DronesDict = { 17 | "Name": (IP, Port), 18 | } 19 | ``` 20 | 21 | ## How to use 22 | 23 | see `test_api.py` as a sample. -------------------------------------------------------------------------------- /telloswarm_plus/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SASLabStevens/TelloSwarm/25af2e88289b9dd8d00b645bed8d0c200b7156b1/telloswarm_plus/__init__.py -------------------------------------------------------------------------------- /telloswarm_plus/mocap.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | Created on Wed Jul 28 14:56:48 2021 6 | 7 | @author: Rayan Bahrami 8 | 9 | Safe Autonomous Systems Lab (SAS Lab) 10 | Stevens Institute of Technology 11 | 12 | 13 | """ 14 | 15 | 16 | import rospy 17 | import tf 18 | 19 | from tf.transformations import euler_from_quaternion, quaternion_from_euler 20 | 21 | import numpy as np 22 | 23 | import time 24 | 25 | 26 | class rosClock: 27 | """ 28 | Description ... 29 | 30 | This object (rosClock) was adopted from TimeHelper object in 31 | https://github.com/USC-ACTLab/crazyswarm 32 | 33 | 34 | """ 35 | 36 | def __init__(self): 37 | self.rosRate = None 38 | self.rateHz = None 39 | 40 | def time(self): 41 | """Returns the current time in seconds.""" 42 | return rospy.Time.now().to_sec() 43 | 44 | def sleep(self, duration): 45 | """Sleeps for the provided duration in seconds.""" 46 | rospy.sleep(duration) 47 | 48 | def sleepForRate(self, rateHz): 49 | """Sleeps so that, if called in a loop, executes at specified rate.""" 50 | if self.rosRate is None or self.rateHz != rateHz: 51 | self.rosRate = rospy.Rate(rateHz) 52 | self.rateHz = rateHz 53 | self.rosRate.sleep() 54 | 55 | def isShutdown(self): 56 | """Returns true if the script should abort, e.g. from Ctrl-C.""" 57 | return rospy.is_shutdown() 58 | 59 | 60 | class MotionCapture: 61 | """ 62 | 63 | TODO: 64 | make the world and child(object) frames configurable. 65 | 66 | currently the default structure is consistent only with the 67 | vicon_bridge pkg @ https://github.com/ethz-asl/vicon_bridge 68 | 69 | """ 70 | 71 | WORLD_FRAME = "/vicon/world" 72 | 73 | def __init__(self, OBJECT_FRAME): 74 | rospy.init_node("GroundTruthPose", anonymous=False, disable_signals=True) 75 | self.tflistener = tf.TransformListener() 76 | self.OBJECT_FRAME = ( 77 | "/vicon" + str("/") + str(OBJECT_FRAME) + str("/") + str(OBJECT_FRAME) 78 | ) 79 | 80 | def getPose(self, mode: str = None): 81 | """ 82 | # http://docs.ros.org/en/jade/api/tf/html/python/transformations.html 83 | 84 | Parameters 85 | ---------- 86 | mode : str, optional 87 | DESCRIPTION. The default is None. 88 | mode == "quaternion", "euler", "None" 89 | 90 | Returns 91 | ------- 92 | TYPE 93 | DESCRIPTION. 94 | euler angles are in radians 95 | 96 | 97 | """ 98 | self.tflistener.waitForTransform( 99 | MotionCapture.WORLD_FRAME, 100 | self.OBJECT_FRAME, 101 | rospy.Time(0), 102 | rospy.Duration(0.03), 103 | ) 104 | position, quaternion = self.tflistener.lookupTransform( 105 | MotionCapture.WORLD_FRAME, self.OBJECT_FRAME, rospy.Time(0) 106 | ) 107 | 108 | if mode == "quaternion": 109 | rotation = quaternion 110 | else: 111 | (Rx, Ry, Rz) = euler_from_quaternion(quaternion) 112 | rotation = (Rx, Ry, Rz) 113 | 114 | # not necessarily: 115 | # (Rx, Ry, Rz) != (roll, pitch, yaw) 116 | 117 | # in list format 118 | # TODO: 119 | 120 | # specify the units of measurement 121 | 122 | # print(position) 123 | # np.array(position) 124 | return np.array(position), np.array(rotation) 125 | 126 | 127 | if __name__ == "__main__": 128 | # rospy.init_node("GroundTruthPose", anonymous=False) 129 | try: 130 | OBJECT_FRAME = input("Enter the object name defined in VICON Tracker: ") 131 | print("Hello", OBJECT_FRAME + "!") 132 | Drone_GroundTruthPose = MotionCapture(str(OBJECT_FRAME)) 133 | 134 | rate = rospy.Rate(100) # in Hz 135 | while not rospy.is_shutdown(): 136 | pos, rot = Drone_GroundTruthPose.getPose() 137 | print("The Position and Rotation are:") 138 | print("position:", pos) 139 | print("orientation:", rot) 140 | print("Press ctrl+C to stop...") 141 | rate.sleep() 142 | 143 | # OBJECT_FRAME = input("Enter the object name defined in VICON Tracker: ") 144 | # print("Hello", OBJECT_FRAME + "!") 145 | # print("The Position and Rotation are:") 146 | # Drone_GroundTruthPose = MotionCapture(str(OBJECT_FRAME)) 147 | # pos, rot = Drone_GroundTruthPose.getPose() 148 | # print(pos) 149 | # print(rot) 150 | # except rospy.ROSInterruptException: pass 151 | except KeyboardInterrupt: 152 | pass 153 | -------------------------------------------------------------------------------- /telloswarm_plus/set-ap-mode.py: -------------------------------------------------------------------------------- 1 | """ python set-ap-mode.py -s [SSID] -p [PASSWORD] 2 | 3 | NOTE: the Wi-Fi network must be 2.4 GHz for Tello EDU to connect to it. 4 | 5 | Ref. https://tello.oneoffcoder.com/swarm.html#set-tello-modes 6 | 7 | 8 | """ 9 | 10 | import socket 11 | import argparse 12 | import sys 13 | 14 | 15 | def get_socket(): 16 | """ 17 | Gets a socket. 18 | :return: Socket. 19 | """ 20 | s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 21 | s.bind(("", 8889)) 22 | 23 | return s 24 | 25 | 26 | def set_ap(ssid, password, address): 27 | """ 28 | A Function to set tello in Access Point (AP) mode. 29 | 30 | :param ssid: The SSID of the network (e.g. name of the Wi-Fi). 31 | :param password: The password of the network. 32 | :param address: Tello IP. 33 | :return: None. 34 | """ 35 | s = get_socket() 36 | 37 | cmd = "command" 38 | print(f"sending cmd {cmd}") 39 | s.sendto(cmd.encode("utf-8"), address) 40 | 41 | response, ip = s.recvfrom(100) 42 | print(f"from {ip}: {response}") 43 | 44 | cmd = f"ap {ssid} {password}" 45 | print(f"sending cmd {cmd}") 46 | s.sendto(cmd.encode("utf-8"), address) 47 | 48 | response, ip = s.recvfrom(100) 49 | print(f"from {ip}: {response}") 50 | 51 | 52 | def parse_args(args): 53 | """ 54 | Parses arguments. 55 | :param args: Arguments. 56 | :return: Arguments. 57 | """ 58 | parser = argparse.ArgumentParser( 59 | "set-ap-mode.py", epilog="setting Tello EDU in AP mode" 60 | ) 61 | 62 | parser.add_argument("-s", "--ssid", help="SSID", required=True) 63 | parser.add_argument("-p", "--pwd", help="password", required=True) 64 | parser.add_argument("--ip", help="Tello IP", default="192.168.10.1", required=False) 65 | parser.add_argument( 66 | "--port", help="Tello port", default=8889, type=int, required=False 67 | ) 68 | parser.add_argument("--version", action="version", version="%(prog)s v0.0.1") 69 | 70 | return parser.parse_args(args) 71 | 72 | 73 | if __name__ == "__main__": 74 | args = parse_args(sys.argv[1:]) 75 | ssid = args.ssid 76 | pwd = args.pwd 77 | tello_address = (args.ip, args.port) 78 | 79 | set_ap(ssid, pwd, tello_address) 80 | -------------------------------------------------------------------------------- /telloswarm_plus/telloserver_EDU.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Mon Jul 26 11:30:08 2021 - modified on May 2024 5 | 6 | @author: Rayan Bahrami 7 | 8 | Safe Autonomous Systems Lab (SAS Lab) 9 | Stevens Institute of Technology 10 | 11 | """ 12 | 13 | import socket 14 | import time 15 | import threading 16 | 17 | import cv2 18 | from telloserver_video import VideoStream 19 | 20 | from dataclasses import dataclass 21 | from typing import Tuple 22 | 23 | 24 | @dataclass 25 | class State: 26 | rotation: Tuple[float, float, float] = (0, 0, 0) 27 | velocity: Tuple[float, float, float] = (0, 0, 0) 28 | acceleration: Tuple[float, float, float] = (0, 0, 0) 29 | altitude: float = 0 30 | barometer: float = 0 31 | time: int = 0 # encapsulated time of the motors running 32 | 33 | 34 | class TelloServer: 35 | """Python API to interact with multiple Tello EDU drones. 36 | It is based on the official SDK 3.0 for Tello EDU drones. 37 | It works with a wifi network (WLAN) to establish unique 38 | Wi-Fi connections with drones, allowing for performing swarming and formation of drones 39 | as well as receiving the drones' onboard data and video stream. 40 | 41 | Tello API's official documentation: 42 | [2](https://dl-cdn.ryzerobotics.com/downloads/Tello/Tello%20SDK%202.0%20User%20Guide.pdf) 43 | [3](https://dl.djicdn.com/downloads/RoboMaster+TT/Tello_SDK_3.0_User_Guide_en.pdf) 44 | 45 | It creates 1 client and 2 servers on 2 threads. 46 | The client sends control commands to the drones. 47 | The two servers on two threads receive the drone's state and the drone's response to the sent commands. 48 | """ 49 | 50 | ## Global Parameters 51 | 52 | # client socket to send commands to the drone 53 | BINDING_IP = "" # server IP - local host 54 | CONTROL_UDP_PORT = 8889 # Tello UDP port 55 | 56 | # server socket to receive the drone's state 57 | STATE_UDP_PORT = 8890 58 | VIDEO_PORT = 11111 59 | 60 | FPS = {5: "low", 15: "middle", 30: "high"} 61 | VIDEO_RES = {"480p": "low", "720p": "high"} 62 | 63 | # mimic TCP 64 | MAX_RETRY = 3 # max number to repeat important commands e.g. land, emergency 65 | 66 | def __init__( 67 | self, 68 | drone_IPs=["192.168.10.1"], 69 | LOCAL_PORT=9000, 70 | ): 71 | """__init__ 72 | 73 | Args: 74 | drone_IPs (list, optional): _description_. Defaults to ["192.168.10.1"]. 75 | The list will be used to create a dictionary of drones states, named self.states with the IPs as keys. 76 | LOCAL_PORT (int, optional): _description_. Defaults to 9000. 77 | 78 | """ 79 | 80 | self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 81 | # self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_BINDTODEVICE, self.bytes_interface) 82 | self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 83 | self.socket.bind(("", LOCAL_PORT)) # server 84 | 85 | # self.address = (self.TELLO_IP, TelloServer.CONTROL_UDP_PORT) 86 | 87 | # thread for receiving cmd ack 88 | self.msg = b"None" # latest message received from the drone (Bytes) 89 | self.client_addr = ["None", 8889] # Tello IP and port 8889 90 | self.recv_msg_thread = threading.Thread(target=self._recv_msg) 91 | self.recv_msg_active = True 92 | self.recv_msg_thread.start() 93 | 94 | self.state_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 95 | self.state_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 96 | # self.state_socket.bind(("", TelloServer.STATE_UDP_PORT)) 97 | self.state_socket.bind(("0.0.0.0", TelloServer.STATE_UDP_PORT)) 98 | 99 | # latest state received from the drones 100 | self.states = {ip: State() for ip in drone_IPs} 101 | self.recv_state_thread = threading.Thread(target=self._recv_state) 102 | self.recv_state_active = True 103 | self.recv_state_thread.start() 104 | 105 | # ===================== drone's response commands ==================== 106 | 107 | def _recv_msg(self): 108 | """_recv_msg _summary_""" 109 | while self.recv_msg_active: 110 | try: 111 | self.msg, self.client_addr = self.socket.recvfrom(3000) 112 | # print(f"client_addr: {client_addr}") # Tello IP and port 8889 113 | # print(f"Received message: {self.msg.decode('utf-8')}") 114 | except socket.error as e: 115 | print(f"Exception socket.error : {e}") 116 | 117 | def _recv_state(self): 118 | """_recv_state callback""" 119 | while self.recv_state_active: 120 | try: 121 | data, addr = self.state_socket.recvfrom(3000) 122 | ( 123 | self.states[addr[0]].rotation, 124 | self.states[addr[0]].velocity, 125 | self.states[addr[0]].acceleration, 126 | self.states[addr[0]].altitude, 127 | self.states[addr[0]].barometer, 128 | self.states[addr[0]].time, 129 | ) = self._decode_state(data) 130 | # tello_IP, tello_PORT = tello_addr[0], tello_addr[1] 131 | # print(f"state of tello {tello_addr}") 132 | # print(f"Received state: {self.state}") 133 | except socket.error as e: 134 | print(f"Exception socket.error : {e}") 135 | 136 | def _decode_state(self, data: bytes): 137 | 138 | # test # data = "mid:-1;x:-100;y:-100;z:-100;mpry:0,0,0;pitch:0;roll:1;yaw:28;vgx:5;vgy:0;vgz:0;templ:69;temph:72;tof:6553;h:80;bat:83;baro:-64.11;time:41;agx:-5.00;agy:-26.00;agz:-1010.00" 139 | 140 | data = data.decode("utf-8") 141 | key_value_pairs = data.split(";") 142 | 143 | rot, vel, acc, altitude, barometer, time = {}, {}, {}, 0, 0, 0 144 | for pair in key_value_pairs: 145 | if pair and ":" in pair: 146 | key, value = pair.split(":") 147 | if key in ["roll", "pitch", "yaw"]: 148 | rot[key] = int(value) 149 | elif key.startswith("vg"): 150 | vel[key] = float(value) / 10 # dm/s to m/s 151 | elif key.startswith("ag"): 152 | acc[key] = float(value) / 100 # cm/s^2 to m/s^2 153 | elif key == "h": 154 | altitude = float(value) / 100 # cm to m 155 | elif key == "baro": 156 | barometer = float(value) # ? see their doc 157 | elif key == "time": 158 | time = int(value) # seconds 159 | 160 | rotation = ( 161 | rot.get("roll", 0), 162 | -rot.get("pitch", 0), 163 | -rot.get("yaw", 0), 164 | ) # degrees in FLU = xyz body-fixed frame 165 | 166 | velocity = ( 167 | vel.get("vgx", 0.0), 168 | -vel.get("vgy", 0.0), 169 | -vel.get("vgz", 0.0), 170 | ) # m/s in FLU = xyz body-fixed frame 171 | 172 | acceleration = ( 173 | acc.get("agx", 0.0), 174 | acc.get("agy", 0.0), 175 | acc.get("agz", 0.0), 176 | ) # m/s^2 177 | 178 | return rotation, velocity, acceleration, altitude, barometer, time 179 | 180 | # ===================== drone's control commands ==================== 181 | 182 | def connect(self, addr): 183 | """ 184 | Enables sending control commands to a Tello drone 185 | """ 186 | self.socket.sendto("command".encode("utf-8"), 0, addr) 187 | 188 | def disconnect(self): 189 | """ 190 | closes the UDP channels used for sending commands to a Tello drone 191 | """ 192 | print(f"Stopping state thread ...") 193 | self.recv_state_active = False 194 | self.recv_state_thread.join() 195 | 196 | print(f"Stopping msg thread ...") 197 | self.recv_msg_active = False 198 | self.recv_msg_thread.join() 199 | 200 | print(f"Closing sockets connections ...") 201 | self.state_socket.close() 202 | self.socket.close() 203 | 204 | def emergency(self, addr): 205 | """stop all motors immediately.""" 206 | self.socket.sendto("emergency".encode("utf-8"), 0, addr) 207 | for _ in range(TelloServer.MAX_RETRY): 208 | print(f"{addr} emergency: {self.msg.decode('utf-8')}") 209 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 210 | break 211 | else: 212 | time.sleep(0.1) 213 | self.socket.sendto("emergency".encode("utf-8"), 0, addr) 214 | 215 | def takeoff(self, addr): 216 | self.socket.sendto("takeoff".encode("utf-8"), 0, addr) 217 | for _ in range(TelloServer.MAX_RETRY): 218 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 219 | print(f"{addr} takeoff: {self.msg.decode('utf-8')}") 220 | break 221 | else: 222 | time.sleep(0.1) 223 | self.socket.sendto("takeoff".encode("utf-8"), 0, addr) 224 | 225 | def land(self, addr): 226 | self.socket.sendto("land".encode("utf-8"), 0, addr) 227 | for _ in range(TelloServer.MAX_RETRY): 228 | print(f"{addr} land: {self.msg.decode('utf-8')}") 229 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 230 | break 231 | else: 232 | time.sleep(0.1) 233 | self.socket.sendto("land".encode("utf-8"), 0, addr) 234 | 235 | def cmdAngleZ(self, roll, pitch, throttle, yawRate, addr): 236 | """ 237 | 238 | - Sends remote control commands in four channels. 239 | - The body frame follows a right-handed Front(x) Left(y) Up(z) (FLU) convention. 240 | 241 | - Roll angle and pitch angle are the setpoints given in the body-frame. 242 | - throttle controls the altitude in the Z axis of the drone's FLU body-frame. 243 | - yaw rate is the rotation rate about the z axis. 244 | 245 | - Frame Convention: 246 | -- Roll angle: cw(-) and ccw(+) rotation about the X axis to move, respectively, in the left and right direction, w.r.t. the FLU body-frame. 247 | -- pitch angle: cw(-) and ccw(+) rotation about the Y axis to move, respectively, in the backward and forward direction, w.r.t. the FLU body-frame. 248 | -- yaw angle: cw(+) and ccw(-) rotation about the Z axis; yes it's negative in the SDK! 249 | 250 | Arguments: 251 | roll (int): percentage of desired roll angle (between -100 to +100 corresponding to -10 to 10 degrees). 252 | pitch (int): percentage of desired pitch angle (between -100 to +100 corresponding to -10 to 10 degrees). 253 | throttle (int): throttle input of the drone (between -100 to +100 corresponding to ~ -100 [cm/sec] to ~ +100 [cm/sec]) 254 | yawRate: -100~100 (ToDo) 255 | 256 | """ 257 | 258 | def clamp(x: int, min_value: int, max_value: int) -> int: 259 | return max(min_value, min(max_value, x)) 260 | 261 | cmd = "rc {} {} {} {}".format( 262 | clamp(round(roll), -100, 100), 263 | clamp(round(pitch), -100, 100), 264 | clamp(round(throttle), -100, 100), 265 | clamp(round(yawRate), -100, 100), 266 | ) 267 | 268 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 269 | 270 | # =============== SDK 3.0 receive the drone's onboard video ================= 271 | 272 | def send_streamon(self, addr): 273 | """Enable video stream""" 274 | self.socket.sendto("streamon".encode("utf-8"), 0, addr) 275 | time.sleep(0.1) 276 | STREAM = False 277 | for _ in range(TelloServer.MAX_RETRY * 2): 278 | print(f"{addr} streamon: {self.msg.decode('utf-8')}") 279 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 280 | STREAM = True 281 | break 282 | else: 283 | time.sleep(0.1) 284 | self.socket.sendto("streamon".encode("utf-8"), 0, addr) 285 | return STREAM 286 | 287 | def set_video_port(self, vid_port, status_port, addr): 288 | cmd = f"port {status_port} {vid_port}" 289 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 290 | VIDEO_PORT = False 291 | for _ in range(TelloServer.MAX_RETRY): 292 | print(f"{addr} setting video port: {self.msg.decode('utf-8')}") 293 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 294 | VIDEO_PORT = True 295 | break 296 | else: 297 | time.sleep(0.1) 298 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 299 | return VIDEO_PORT 300 | 301 | def streamoff(self, addr): 302 | """Disable video stream""" 303 | self.socket.sendto("streamoff".encode("utf-8"), 0, addr) 304 | for _ in range(TelloServer.MAX_RETRY * 2): 305 | print(f"{addr} streamoff: {self.msg.decode('utf-8')}") 306 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 307 | break 308 | else: 309 | time.sleep(0.1) 310 | self.socket.sendto("streamoff".encode("utf-8"), 0, addr) 311 | 312 | def set_video_fps(self, fps: int, addr): 313 | """set video stream FPS to 5, 15, or 30""" 314 | cmd = f"setfps {TelloServer.FPS[fps]}" 315 | 316 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 317 | for _ in range(TelloServer.MAX_RETRY): 318 | print(f"{addr} setting FPS: {self.msg.decode('utf-8')}") 319 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 320 | break 321 | else: 322 | time.sleep(0.1) 323 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 324 | 325 | def set_video_resolution(self, res: str, addr): 326 | """set video res. to 480p or 720""" 327 | cmd = f"setresolution {TelloServer.VIDEO_RES[res]}" 328 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 329 | for _ in range(TelloServer.MAX_RETRY): 330 | print(f"{addr} setting video resolution: {self.msg.decode('utf-8')}") 331 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 332 | break 333 | else: 334 | time.sleep(0.1) 335 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 336 | 337 | def set_video_bitrate(self, rate, addr): 338 | """set video bitrate to 339 | 0: auto 340 | 1: 1Mbps 341 | 2: 2Mbps 342 | 3: 2Mbps 343 | 4: 3Mbps 344 | 5: 4Mbps 345 | """ 346 | cmd = f"setbitrate {rate}" 347 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 348 | for _ in range(TelloServer.MAX_RETRY): 349 | print(f"{addr} setting video bitrate: {self.msg.decode('utf-8')}") 350 | if "ok" in self.msg.decode("utf-8") and self.client_addr[0] == addr[0]: 351 | break 352 | else: 353 | time.sleep(0.1) 354 | self.socket.sendto(cmd.encode("utf-8"), 0, addr) 355 | 356 | 357 | # =============== Multi-agent API for swarm and formation ================ 358 | 359 | # 360 | from concurrent.futures import ThreadPoolExecutor 361 | 362 | 363 | class TelloSwarm: 364 | """ 365 | Multi-agent API for swarm and formation 366 | 367 | """ 368 | 369 | def __init__(self, dronesDict, mocap=False): 370 | """ 371 | 372 | Parameters 373 | ---------- 374 | wifi_interfaces : list 375 | DESCRIPTION. 376 | GT 377 | droneslist : str = optional 378 | DESCRIPTION. 379 | 380 | defaultName : str, optional 381 | DESCRIPTION. The default is 'Drone'. 382 | 383 | Returns 384 | ------- 385 | None. 386 | 387 | """ 388 | # self.droneslist = [] 389 | # for drone in dronesDict: 390 | # self.TELLO_IP = dronesDict[drone] # Tello IP address 391 | # self.Drone_Name = drone # Tello name 392 | # self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 393 | # self.socket.bind((TelloServer.BINDING_IP, TelloServer.CONTROL_UDP_PORT)) 394 | # self.address = (self.TELLO_IP, TelloServer.CONTROL_UDP_PORT) 395 | 396 | self.droneslist = [] # just the list of names 397 | self.dronesDict = dronesDict 398 | 399 | self.IPs = [addr[0] for addr in list(dronesDict.values())] 400 | self.video_ports = [addr[1] for addr in list(dronesDict.values())] 401 | 402 | # 1 client and 2 servers on 2 threads 403 | self.tellos = TelloServer(drone_IPs=self.IPs) 404 | 405 | # N video streams over UDP on N threads 406 | self.cameras = {ip: None for ip in self.IPs} 407 | self.STREAM = False 408 | 409 | self.mocap = mocap 410 | if self.mocap: 411 | from mocap import MotionCapture # , rosClock 412 | 413 | self.GroundTruth = { 414 | addr[0]: MotionCapture(name) for name, addr in self.dronesDict.items() 415 | } 416 | 417 | # ============= control commands for the swarm ================= 418 | # TCP-like commands to start and stop the drones in the swarm 419 | def connect(self): 420 | for ip in self.IPs: 421 | addr = (ip, 8889) 422 | self.tellos.connect(addr) 423 | 424 | def disconnect(self, streamoff=True): 425 | if self.STREAM and streamoff: 426 | for ip in self.IPs: 427 | addr = (ip, 8889) 428 | # if self.cameras[ip] is not None: 429 | self.tellos.streamoff(addr) 430 | 431 | cv2.destroyAllWindows() 432 | self.tellos.disconnect() 433 | 434 | def takeoff(self): 435 | for ip in self.IPs: 436 | addr = (ip, 8889) 437 | self.tellos.takeoff(addr) 438 | 439 | def land(self, delay=3): 440 | for ip in self.IPs: 441 | addr = (ip, 8889) 442 | self.tellos.land(addr) 443 | time.sleep(delay) 444 | 445 | def emergency(self): 446 | for ip in self.IPs: 447 | addr = (ip, 8889) 448 | self.tellos.emergency(addr) 449 | 450 | # drone control commands to be called for each drone 451 | def cmdAngleZ(self, drone_IP: str, roll, pitch, throttle, yawRate): 452 | addr = (drone_IP, 8889) 453 | # for whatever reason, yawRate is negative FLU (xyz) coord. frame in the SDK 454 | self.tellos.cmdAngleZ(roll, pitch, throttle, -yawRate, addr) 455 | 456 | # ============= receive drones' onboard data ================= 457 | def get_state(self, drone_IP): 458 | """get_state 459 | 460 | Args: 461 | drone_IP (str): drone's IP address on the network 462 | 463 | Returns: a dataclass object with the following attributes: 464 | rotation = (roll, pitch, yaw) in degrees and FLU body-fixed frame 465 | velocity = (vx, vy, vz), 466 | acceleration = (ax, ay, az), 467 | altitude, barometer, time_of_flight 468 | """ 469 | return self.tellos.states[drone_IP] 470 | 471 | def streamon(self, bufsize=5, FPS=None, resolution=(None, None), resize=False): 472 | """activate video stream from Tello drones 473 | 474 | Args: 475 | bufsize : int, 476 | FPS : int, frame per second (5 - 30), optional 477 | resolution (H=720,W=960) : tuple, optional 478 | resize : True or False, optional - make it True if you get distorted images for lower resolution 479 | """ 480 | for id, ip in enumerate(self.IPs): 481 | addr = (ip, 8889) 482 | PORT = self.tellos.set_video_port(self.video_ports[id], 8890, addr) 483 | STREAM = self.tellos.send_streamon(addr) 484 | 485 | self.STREAM = True 486 | if STREAM and PORT: 487 | self.cameras[ip] = VideoStream( 488 | ip, 489 | self.video_ports[id], 490 | bufsize, 491 | FPS, 492 | resolution, 493 | resize, 494 | ) 495 | if not self.cameras[ip].video.isOpened(): 496 | self.STREAM = False # streamon failed 497 | break 498 | else: 499 | print(f"streamon failed.") 500 | self.STREAM = False # streamon failed 501 | break 502 | 503 | def get_frame(self, drone_IP): 504 | """get_frame assuming the video stream is on""" 505 | return self.cameras[drone_IP].frame 506 | 507 | def streamoff(self): 508 | """disable video stream asuming the video stream is on""" 509 | for ip in self.IPs: 510 | addr = (ip, 8889) 511 | if self.cameras[ip].video.isOpened(): 512 | self.cameras[ip].stop() # stops telloserver_video.py 513 | self.tellos.streamoff(addr) 514 | 515 | def set_video_fps(self, fps: int): 516 | """set video stream FPS to 5, 15, or 30""" 517 | for ip in self.IPs: 518 | addr = (ip, 8889) 519 | self.tellos.set_video_fps(fps, addr) 520 | 521 | def set_video_resolution(self, res: str): 522 | """set video res. to 480p or 720""" 523 | for ip in self.IPs: 524 | addr = (ip, 8889) 525 | self.tellos.set_video_resolution(res, addr) 526 | 527 | def set_video_bitrate(self, rate=0): 528 | """set video bitrate""" 529 | for ip in self.IPs: 530 | addr = (ip, 8889) 531 | self.tellos.set_video_bitrate(rate, addr) 532 | 533 | def get_mocap_groundtruth_pose(self, drone_IP, mode: str = None): 534 | """get_mocap_groundtruth_pose assuming self.mocap is True 535 | Args: 536 | drone_IP (str): 537 | mode (str, optional): "quaternion", "euler" = "None". Defaults to None. 538 | 539 | Returns: 540 | position, rotation: np.array of pos=(px, py, pz) in world FLU, and (roll, pitch, yaw) in radians 541 | """ 542 | return self.GroundTruth[drone_IP].getPose(mode) 543 | 544 | # the landing function was written by Shalemuraju Katari @ https://github.com/SASLabStevens/Swarm-Drones-Using-Shape-Vectors 545 | # The landing function was not meant to be optimum; modification and rectification may be required! 546 | # @staticmethod 547 | def Landing(GroundTruth, allDrones, rosClock): 548 | """ 549 | a landing function that uses motion capture data 550 | this is used to land the standard Tello drones safely on the ground. 551 | for Tello EDU drones, use land() method in TelloSwarm class. 552 | 553 | Arguments: 554 | ---------- 555 | GT : GroundTruth object ( the output of MotionCaptureGroundTruth method in SWARM class) 556 | allDrones: the list of drones generated by __init__ method in SWARM class 557 | """ 558 | GT = GroundTruth 559 | # print('Received Landing Command') 560 | safe_height = False 561 | 562 | while safe_height == False: 563 | try: 564 | 565 | altitude = [] 566 | for drone in GT: 567 | altitude.append(drone.getPose()[0][2]) 568 | count = 0 569 | 570 | for drone in allDrones: 571 | idx = allDrones.index(drone) 572 | if altitude[idx] > 0.07: 573 | 574 | landing_velocity = ( 575 | altitude[idx] * 10 if altitude[idx] < 0.035 else 60 576 | ) 577 | drone.cmdAngleZ(0, 0, -landing_velocity, 0) 578 | else: 579 | drone.emergency() 580 | rosClock.sleepForRate(10) 581 | drone.emergency() 582 | count += 1 583 | 584 | if count == len(altitude): 585 | safe_height = True 586 | 587 | except KeyboardInterrupt: 588 | 589 | print("emergency interruption!; aborting all flights ...") 590 | for drone in allDrones: 591 | drone.emergency() 592 | 593 | rosClock.sleepForRate(10) 594 | -------------------------------------------------------------------------------- /telloswarm_plus/telloserver_video.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Mon Jul 26 11:30:08 2021 - modified on May 2024 5 | 6 | @author: Rayan Bahrami 7 | 8 | Safe Autonomous Systems Lab (SAS Lab) 9 | Stevens Institute of Technology 10 | 11 | """ 12 | 13 | import threading 14 | import cv2 15 | 16 | 17 | class VideoStream: 18 | """ 19 | Python class to stream video from multiple Ryze Tello drones. 20 | It is writen to be used along with the TelloServerEDU class. 21 | It requires the Ryze Tello SDK 3.0 to be installed on Tello. 22 | [2](https://dl-cdn.ryzerobotics.com/downloads/Tello/Tello%20SDK%202.0%20User%20Guide.pdf) 23 | [3](https://dl.djicdn.com/downloads/RoboMaster+TT/Tello_SDK_3.0_User_Guide_en.pdf) 24 | It uses OpenCV to display the video stream. 25 | """ 26 | 27 | def __init__( 28 | self, 29 | TELLO_IP: str, # default is "0.0.0.0" for a single drone 30 | VIDEO_PORT: int, # 11111 31 | bufsize=5, 32 | FPS=None, # max is 30 33 | resolution=(None, None), 34 | resize=False, # make it True if you get distorted images for lowe resolution 35 | ): 36 | """ 37 | 38 | Parameters 39 | ---------- 40 | TELLO_IP : str, IP address of the Tello drone on the wifi network 41 | VIDEO_PORT : int, port number on the PC for video stream (default is 11111). This is configurable with TelloServer.set_video_port() 42 | FPS : int, frame per second (5 - 30), optional 43 | resolution (H=720,W=960) : tuple, optional 44 | 45 | Returns 46 | ------- 47 | None. 48 | 49 | """ 50 | self.TELLO_IP = TELLO_IP 51 | self.VIDEO_PORT = VIDEO_PORT 52 | self.bufsize = bufsize 53 | self.FPS = 30 if FPS is None else FPS 54 | self.resolution = resolution 55 | self.resize = resize 56 | 57 | self.frame = b"None" # latest frame received from the drone camera 58 | # self.lock = threading.Lock() 59 | 60 | # Initialize video stream from Tello 61 | 62 | print(f"Connecting to cv2.VideoCapture(), it may take ~30 seconds ...") 63 | # https://docs.opencv.org/4.8.0/dd/d43/tutorial_py_video_display.html 64 | self.video = cv2.VideoCapture( 65 | f"udp://{self.TELLO_IP}:{self.VIDEO_PORT}", cv2.CAP_ANY 66 | ) 67 | if not self.video.isOpened(): 68 | # self.video_isOpened = False 69 | print("Error: VideoCapture not opened!, exit ...") 70 | 71 | else: 72 | # self.video_isOpened = True 73 | print(f"Connected! streaming on cv2.VideoCapture() ... ") 74 | 75 | self.video.set(cv2.CAP_PROP_BUFFERSIZE, self.bufsize) 76 | if FPS is not None: 77 | self.video.set(cv2.CAP_PROP_FPS, self.FPS) 78 | if resolution[0] is not None and resize is False: 79 | self.video.set(cv2.CAP_PROP_FRAME_HEIGHT, self.resolution[0]) 80 | if resolution[1] is not None and resize is False: 81 | self.video.set(cv2.CAP_PROP_FRAME_WIDTH, self.resolution[1]) 82 | 83 | self.recv_video_thread = threading.Thread(target=self._recv_video) 84 | self.recv_video_active = True 85 | self.recv_video_thread.start() 86 | 87 | def _recv_video(self): 88 | """get video stream from Tello""" 89 | while self.recv_video_active: 90 | try: 91 | ret, frame = self.video.read() # capture frame-by-frame 92 | if ret: # if frame is read correctly 93 | if self.resize and self.resolution[0] is not None: 94 | frame = cv2.resize(frame, self.resolution[::-1]) 95 | # self.frame = cv2.resize(frame, (0, 0), fx=0.5, fy=0.5) 96 | # with self.lock: 97 | self.frame = frame 98 | # cv2.imshow(f"{self.TELLO_IP}", self.frame) # bad idea here! 99 | # cv2.imwrite(f"{self.TELLO_IP}_at_{time.time()}.png", self.frame) 100 | # cv2.waitKey(1) 101 | 102 | except Exception as e: 103 | print(f"Exception cv2.VideoCapture() error : {e}") 104 | 105 | # ===================== drone's control commands ==================== 106 | 107 | def stop(self): 108 | """ 109 | closes the UDP channels used for sending commands to a Tello drone 110 | """ 111 | self.recv_video_active = False 112 | self.video.release() 113 | cv2.destroyAllWindows() 114 | self.recv_video_thread.join() 115 | -------------------------------------------------------------------------------- /telloswarm_plus/test_api.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # -*- coding: utf-8 -*- 4 | """ 5 | Created on Mon Jul 26 11:30:08 2021 - modified on May 2024 6 | 7 | @author: Rayan Bahrami 8 | 9 | Safe Autonomous Systems Lab (SAS Lab) 10 | Stevens Institute of Technology 11 | 12 | """ 13 | 14 | from telloserver_EDU import TelloSwarm 15 | import time 16 | import cv2 17 | 18 | DronesDict = { 19 | # "Name_in_the_VICON_Tracker": ("IP_on_router", "Port_for_Video_stream"), 20 | # "EDU": ("192.168.10.1", 11111), factory default 21 | "EDU_7": ("172.16.0.17", 1037), # drone IP on router, port for Video stream 22 | "EDU_8": ("172.16.0.18", 1038), 23 | # "EDU_9": ("172.16.0.19", 1039), 24 | } 25 | 26 | Ts, max_itr = 0.02, 100 27 | 28 | # fmt: off 29 | swarm = TelloSwarm(DronesDict, mocap=False) # mocap=True if you have vicon_bridge running running 30 | swarm.connect() 31 | 32 | swarm.set_video_bitrate(3) # 0: auto, 1: 1Mbps, 2: 2Mbps, 3: 3Mbps, 4: 4Mbps, 5: 5Mbps 33 | swarm.set_video_fps(30) 34 | swarm.set_video_resolution("480p") # 480p, 720p, 35 | 36 | swarm.streamon(bufsize=10, FPS=30, resolution=(480, 640), resize=True) # (480, 640), (None, None) = (720,960) = (H, w) 37 | 38 | if swarm.STREAM: 39 | swarm.takeoff() 40 | 41 | time.sleep(2) 42 | 43 | i = 0 44 | while i <= max_itr and swarm.STREAM: 45 | i += 1 46 | time.sleep(Ts) 47 | for id, name in enumerate(DronesDict): 48 | IP, _ = DronesDict[name] 49 | print(f"drone {name}") 50 | swarm.cmdAngleZ(IP, 0, 0, 0, 50) 51 | if i % 5 == 0: 52 | states = swarm.get_state(IP) 53 | print(f"drone {name} state: {states}") 54 | # pos, rot = swarm.get_mocap_groundtruth_pose(IP) 55 | # print(f"mocap {name} pos: {pos}, rot: {rot}") 56 | try: 57 | frame = swarm.get_frame(IP) 58 | cv2.imwrite(f"{name}_{time.time()}.png", frame) 59 | cv2.imshow(f"{name}", frame) # swarm.cameras[IP].frame 60 | cv2.waitKey(1) 61 | except: 62 | print(f"Error: {name} cv2.imshow() failed") 63 | continue 64 | 65 | 66 | swarm.land() 67 | 68 | swarm.disconnect() 69 | --------------------------------------------------------------------------------