From 2edfc4fc126664fc34296605865de1ea2338eceb Mon Sep 17 00:00:00 2001 From: elcstazi Date: Sun, 26 May 2024 14:52:28 +0300 Subject: [PATCH 1/3] drone control, server interaction --- .../solutions/following/swarm_controlls.py | 228 ++++++++++++++---- 1 file changed, 187 insertions(+), 41 deletions(-) diff --git a/swarm/scripts/solutions/following/swarm_controlls.py b/swarm/scripts/solutions/following/swarm_controlls.py index 65732bd..0b71985 100644 --- a/swarm/scripts/solutions/following/swarm_controlls.py +++ b/swarm/scripts/solutions/following/swarm_controlls.py @@ -12,6 +12,10 @@ from swarm.scripts.solutions.following.state import State from utils.write_text import write_status_on_image + +from jsonrpcclient import request +import requests + def dist_beetwen_points(p1, p2): return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2) @@ -28,23 +32,50 @@ def __init__(self): self.num_of_rotates = 0 self.flag = 0 self.flag_two = 0 - # self.drone_id = None - # self.get_drone_id() self.drone_id = 1 self.coord_drone = None self.num_of_rotate = 0 + self.swarm_coordinated = False + self.coord_drone = None + + self.rotate_flag = 1 + self.move_flag = 0 + self.move_marker = 0 + def stop(self): self.is_stopped = True - # def get_drone_id(self): - # client = docker.client.from_env() - # self.drone_id = os.environ.get('HOSTNAME') - # if not self.drone_id: - # self.drone_id = -1 + def move_to_marker(self): + x, y, z =self.move_marker + x_un = self.coord_drone[0] - x + y_un = self.coord_drone[1] - y + z_un = self.coord_drone[2] - z + self.coord_drone = (x_un, y_un, z_un) + coord_drone = (x_un, y_un, z_un) + angle_degrees = 360 - self.num_of_rotates + + # Получаем матрицу поворота из вектора ориентации + angle_radians = math.radians(angle_degrees) + + # Преобразуем угол поворота в матрицу поворота + R = np.array([[np.cos(angle_radians), -np.sin(angle_radians), 0], + [np.sin(angle_radians), np.cos(angle_radians), 0], + [0, 0, 1]]) + + # Исходные координаты дрона + drone_coords = np.array(coord_drone) + + # Применяем поворот к координатам дрона + rotated_coords = np.dot(R, drone_coords) + # Добавляем координаты нулевой позиции дрона + x = int(rotated_coords[0] - 10) + y = int(rotated_coords[1]) + z = int(rotated_coords[2]) + return (State.MOVE, np.array([x, y, z], dtype=np.int32)) def find_marker(self, frame): corners, ids, rejected = cv2.aruco.detectMarkers(frame, ARUCO_DICT, parameters=ARUCO_PARAMS) @@ -60,18 +91,62 @@ def find_marker(self, frame): def make_action(self, m_corner): self.last_time = time.process_time() - rvecs , tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(np.array([m_corner]), MARKER_LEN, camera_matrix, camera_distortion) + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(np.array([m_corner]), MARKER_LEN, camera_matrix, + camera_distortion) state = self.get_state(tvecs[0][0]) if state == State.MOVE: - self.actions.put(state) + self.actions.put((state, None)) self.last_state = state - + def marker_coords(self, m_corner, marker_id): + self.last_time = time.process_time() + _, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(np.array([m_corner]), MARKER_LEN, camera_matrix, + camera_distortion) + tvec = tvecs[0][0] + marker_x, marker_y, marker_z = tvec[2], tvec[0], tvec[1] + x = z = y = 0 + drone_x = x + marker_x + drone_y = y + marker_y + drone_z = z + marker_z + print(drone_x, drone_y, drone_x) + angle_radians = math.radians(self.num_of_rotates) + + # Преобразуем угол поворота в матрицу поворота + R = np.array([[np.cos(angle_radians), -np.sin(angle_radians), 0], + [np.sin(angle_radians), np.cos(angle_radians), 0], + [0, 0, 1]]) + + # Исходные координаты дрона + drone_coords = np.array([drone_x, drone_y, drone_z]) + + # Применяем поворот к координатам дрона + rotated_coords = np.dot(R, drone_coords) + + # Добавляем координаты нулевой позиции дрона + drone_x_zero = int(rotated_coords[0]) + drone_y_zero = int(rotated_coords[1]) + drone_z_zero = int(rotated_coords[2]) + + # Добавляем смещение для учета положения дрона относительно маркера + if marker_id == MARKER_CENTER: + x = y = z = 0 # may be z = 50 + x = x + drone_x_zero + y = y + drone_y_zero + z = z + drone_z_zero + self.coord_drone = (x, y, z) + print(f"{self.drone_id}, {x}, {y}, {z}\n") + elif marker_id == MARKER_MOVE: + x = y = z = 0 # may be z = 50 + x = x + drone_x_zero + self.coord_drone[0] + y = y + drone_y_zero + self.coord_drone[1] + z = z + drone_z_zero + self.coord_drone[2] + self.move_marker = (x,y,z) + return (x, y, z) def process_frame(self, frame): ids = None is_find = True - if self.last_state == State.READY: + if self.last_state == State.READY and self.flag == int(self.swarm_coordinated): print("State.READY") ret = self.find_marker(frame) if ret is not None: @@ -82,58 +157,98 @@ def process_frame(self, frame): if marker_id == MARKER_CENTER and not self.flag: print("Send COORDS") self.marker_coords(marker_corner, MARKER_CENTER) - self.flag = 1 print("START LINE COORDS") print("degree rotate - ",self.num_of_rotates) - # i=1 - # for _ in range(self.num_of_rotates, 360, 45): - self.num_of_rotate = 360 - self.num_of_rotates - self.actions.put((State.ROTATE, None)) - # print(i) - # i+=1 - # self.num_of_rotates = 0 + # vvvvvvvvvvvv ОПРЕДЕЛИТЬ КОРРЕКТНЫЙ ИРЛ!!! vvvvvvvvvvvvvvvv + url = f"http://{""}:65001" + params = { + "command": "turn_starting_position", + "coordinates": self.coord_drone + } + req = request("exec", params) + response = requests.post(url, json=req) + # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ print("END LINE COORDS") - - if self.flag == 1: + if self.flag == 1 and self.rotate_flag == 0: if marker_id == MARKER_MOVE: - self.marker_coords(marker_corner, MARKER_MOVE) + coord = self.marker_coords(marker_corner, MARKER_MOVE) + # print(мы нашили маркер мув и угол) + url = f"http://{""}:65001" + params = { + "command": "save_coord_move_marker", + "coordinates": coord + } + req = request("exec", params) + response = requests.post(url, json=req) + if self.flag == 1: + if self.move_flag == 1: state = self.move_to_marker() self.actions.put(state) self.last_state = state - + self.move_flag = 0 if marker_id == MARKER_STOP: print("Send END") - self.actions.put((State.END, None)) - self.last_state = State.END + # vvvvvvvvvvvv ОПРЕДЕЛИТЬ КОРРЕКТНЫЙ ИРЛ!!! vvvvvvvvvvvvvvvv + url = f"http://{""}:65001" + params = { + "command": "land" + } + req = request("exec", params) + response = requests.post(url, json=req) + # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ if marker_id == MARKER_CENTER: + if self.rotate_flag == 1: + print("Send ROTATE 2") + self.actions.put((State.ROTATE, None)) + self.last_state = State.ROTATE + self.rotate_flag = 0 + else: + url = f"http://{""}:65001" + params = { + "command": "set_rotate" + } + req = request("exec", params) + response = requests.post(url, json=req) + else: + if self.rotateflag == 1: print("Send ROTATE 2") self.actions.put((State.ROTATE, None)) self.last_state = State.ROTATE - else: - print("Send ROTATE 2") - self.actions.put((State.ROTATE, None)) - self.last_state = State.ROTATE + if self.flag == 1: + self.rotate_flag = 0 + else: - # if not self.flag: print("Send ROTATE 1") - self.actions.put((State.ROTATE, None)) - self.last_state = State.ROTATE + if self.rotate_flag == 0: + url = f"http://{""}:65001" + params = { + "command": "set_rotate" + } + req = request("exec", params) + response = requests.post(url, json=req) + if self.rotate_flag == 1: + self.actions.put((State.ROTATE, None)) + self.last_state = State.ROTATE + if self.flag == 1: + self.rotate_flag = 0 if time.process_time() - self.last_time > TIMEOUT: print("Send END2") - self.actions.put((State.END, None)) - self.last_state = State.END - # time.sleep(1.5) + url = f"http://{""}:65001" + params = { + "command": "land", + } + req = request("exec", params) + response = requests.post(url, json=req) + write_status_on_image(frame, {"Battery": f"{self.last_battery_charge}%", "Last state": self.last_state}) self.video_writer.write(cv2.resize(frame, FRAME_SIZE)) return frame def drone_instructions(self, tello): tello.takeoff() - # tello.move_up(50) self.last_time = time.process_time() self.last_state = State.READY - # x = z = y = tello.get_current_xyz() while not self.is_stopped: try: @@ -142,7 +257,6 @@ def drone_instructions(self, tello): if state == State.END: break elif state == State.MOVE: - # print("DO MOVE") x, y, z = movements print(movements) tello.go_xyz_speed(int(x), int(y), int(z), SPEED) @@ -154,20 +268,52 @@ def drone_instructions(self, tello): tello.rotate_clockwise(x) self.num_of_rotates += x print("Rotate Degree", self.num_of_rotates) + + self.last_time -= 5 else: x = self.num_of_rotate tello.rotate_clockwise(x) self.num_of_rotate = 0 print("Rotate on first pose", self.num_of_rotate) self.num_of_rotates = 0 - - - except Empty: self.last_state = State.READY self.last_battery_charge = tello.get_battery() tello.land() + def turn_starting_position(self, coord): + if not self.flag and self.coord_drone == coord: + self.flag = 1 + self.num_of_rotate = 360 - self.num_of_rotates + self.actions.put((State.ROTATE, None)) + self.last_state = State.ROTATE + self.rotate_flag = 0 + return "OK" + + + def land(self): + self.actions.put((State.END, None)) + self.last_state = State.END + return "OK" + + def set_swarm_is_coordinated(self): + self.swarm_coordinated = True + return "OK" + + def set_rotate_flag(self): + self.rotate_flag = 1 + return "OK" + + def save_move_marker(self, coord): + self.move_flag = 1 + self.move_marker = coord + return "OK" + + def save_coord_all_drones(self, coord_all_drones): + self.coord_all_drones = coord_all_drones + return "OK" + + def main(self): self.is_stopped = False drone = Drone(self.drone_instructions, self.process_frame, show_stream=True) From cff8bfc6287d672a349917b394ce158a8da221e9 Mon Sep 17 00:00:00 2001 From: Kocheshkov Alexander <88431323+KocheshkovAA@users.noreply.github.com> Date: Tue, 28 May 2024 04:07:44 +0500 Subject: [PATCH 2/3] Update README.md --- README.md | 73 ++++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 72 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0af8757..16dacf0 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,76 @@ # Итерация 4 -Из-за необходимости тестирования кода в лаборатории, а также сокращении итерации, проект не удалось закончить к дедлайну 22.05. Проект будет завершен в понедельник (27.05) или же в пятницу (24.05), если лаборатория будет открыта. +В результате итерации не удалось запустить рой. Исправлены недочеты прошлых итераций. +Зависимости: +Версия ОС: Ubuntu 22.04.4 LTS +Python 3.10.12 + +## Конфигурирование +Заполнить [конфигурационный файл](https://github.com/OSLL/tello-dev/blob/master/swarm/networks.json) +```json +{ + "ifaces" : { + "iface-name" : { + "ssid" : "", + "password" : "", + "ip": "172.18.0.3" + }, + "iface-name": { + "ssid" : "", + "password" : "", + "ip": "172.18.0.4" + } + } +} +``` +- Необходимо выполнить команду `ip a` и вставить имена необходимых wlan-интерфейсов вместо `iface-name`. +- Установить пакет `network-manager`: + `apt install network-manager=1.36.6-0ubuntu2` +- Выполнить команду `nmcli dev wifi list` и определить SSID wifi сетей причастных к дрону и вставить вместо `ssid` + +**Пример сконфигурированного файла** +```json +{ + "ifaces" : { + "wlo0" : { + "ssid" : "DRONE 1", + "password" : "", + "ip": "172.18.0.3" + }, + "wlo1": { + "ssid" : "DRONE 2", + "password" : "", + "ip": "172.18.0.4" + } + } +} +``` + +## Запуск +Установить зависимости +```bash +python3 -m pip install jsonrpcclient==4.0.3 requests==2.31.0 +``` +- Необходимо выполнить команду `docker compose -f ./swarm/docker-compose.yml up -d --build` (в одном месте монтируется с хоста папка и этого не избежать, иначе из контейнера нельзя будет управлять сетевыми устройствами) +- После запуска необходимо вызвать скрипт *client.py* передав флаги -c --ip , + где: + + --ip - P-адреса сервера JSON-RPC, к которому скрипт будет отправлять запросы, у нас это 127.0.0.1. + + -c - команда для дронов. + +Пример: +```bash +python3 ./swarm/scripts/rpc/client.py -c takeoff --ip 127.0.0.1 +``` +Результат: +```bash +RESULT for 'http://127.0.0.1:65001' : + {'jsonrpc': '2.0', 'result': 'OK', 'id': 1} +``` +## Презентация и видео +Презентация:[ОУПРПО. Итерация 4. Проект 12](https://docs.google.com/presentation/d/15QLL1ix36oWb84pD0Ya88piNtv9CHaV_Dnr0AQvBDYY/edit?usp=sharing) + + # Итерация 3 Зависимости: Версия ОС: Ubuntu 22.04.4 LTS From 8b1e1ac539e499b517a58f92e9298e56c34e7ddc Mon Sep 17 00:00:00 2001 From: Kocheshkov Alexander <88431323+KocheshkovAA@users.noreply.github.com> Date: Tue, 28 May 2024 14:39:19 +0500 Subject: [PATCH 3/3] Update README.md --- README.md | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 16dacf0..765105c 100644 --- a/README.md +++ b/README.md @@ -50,7 +50,7 @@ Python 3.10.12 ```bash python3 -m pip install jsonrpcclient==4.0.3 requests==2.31.0 ``` -- Необходимо выполнить команду `docker compose -f ./swarm/docker-compose.yml up -d --build` (в одном месте монтируется с хоста папка и этого не избежать, иначе из контейнера нельзя будет управлять сетевыми устройствами) +- Необходимо выполнить команду `docker compose --file ./swarm/docker-compose.yml up -d --build` (в одном месте монтируется с хоста папка и этого не избежать, иначе из контейнера нельзя будет управлять сетевыми устройствами) - После запуска необходимо вызвать скрипт *client.py* передав флаги -c --ip , где: @@ -67,7 +67,16 @@ python3 ./swarm/scripts/rpc/client.py -c takeoff --ip 127.0.0.1 RESULT for 'http://127.0.0.1:65001' : {'jsonrpc': '2.0', 'result': 'OK', 'id': 1} ``` -## Презентация и видео +## Остановка контейнеров +```bash +docker compose --file ./swarm/docker-compose.yml stop +``` +## Удаление контейнеров +```bash +docker compose --file ./swarm/docker-compose.yml down +``` + +## Презентация Презентация:[ОУПРПО. Итерация 4. Проект 12](https://docs.google.com/presentation/d/15QLL1ix36oWb84pD0Ya88piNtv9CHaV_Dnr0AQvBDYY/edit?usp=sharing)