ФАЙЛ «CONTACT_CONTROLLER.PY»

ИНИЦИАЛИЗАЦИЯ БИБЛИОТЕК

import asyncio
import json
import logging
import socket
import matplotlib.pyplot as plt
import numpy as np
import math
import asyncio
from plotly.utils import PlotlyJSONEncoder
import aiofiles
import gpsd
from geopy.distance import geodesic
import plotly.graph_objs as go

from Math import Math
from Frenet_frame import Run_Robot

asyncio - библиотека в Python для написания асинхронного кода.

json - библиотека для работы с данными в формате JSON.

logging - библиотека для ведения логов в Python.

socket - библиотека для работы с сокетами в Python.

matplotlib.pyplot - библиотека для построения графиков в Python.

numpy - библиотека для работы с массивами и матрицами в Python.

math - модуль в Python, предоставляющий математические функции.

aiofiles - библиотека для работы с файлами в асинхронном режиме.

gpsd - библиотека для работы с GPS данными в Python.

geopy.distance - библиотека для вычисления расстояний между двумя точками на Земле.

plotly.graph_objs - библиотека для создания графиков и диаграмм с использованием Plotly.

КЛАСС «ROBOT_CONTROLLER»

class RobotController:
    async def perform_movement(self, distance_cm):
        raise NotImplementedError

    async def perform_spin_angle(self, angle):
        raise NotImplementedError

    async def navigate_to_marker(self, goal_id):
        raise NotImplementedError

Данный класс используется для управления движением и поворотами ровера. Содержит в себе следующие методы:

perform_movement: перемещение на определенное расстояние в сантиметрах.

perform_spin_angle: поворот на определенный градус.

navigate_to_marker: навигация к маркеру или цели.

Данный класс используется для определения поведения и стратегии движения ровера в различных сценариях и задачах.

КЛАСС «SIMULATION CONTROLLER»

Данный класс является симулятором движений ровера. Все действия, прописанные в этом классе, выполняются в двумерном графике в виде отдельного окна Python.

КОНСТРУКТОР КЛАССА

def __init__(self, aruco_map, robot_state, ax,HOST_SOCKET='loclalhost',PORT_SOCKET=5530):
        """
        Конструктор класса.

        Параметры:
        - aruco_map (dict): Карта с координатами маркеров ArUco.
        - robot_state (ContactState): Состояние робота (координаты и ориентация).
        - ax (matplotlib.axes.Axes): Объект matplotlib для отображения графики.

        Описание:
        Инициализирует карту маркеров ArUco, состояние робота, графический объект matplotlib и строит граф смежности маркеров.
        Также отображает карту маркеров на графике и добавляет маркер для позиции робота и линию для отображения пути.
        """
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # создание сокета для сайта
        self.client_socket.connect((HOST_SOCKET, PORT_SOCKET)) # подключение к сайту
        self.client_socket.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 65536) # постановка буфера
        self.aruco_map = aruco_map  # Карта с координатами маркеров ArUco
        self.robot_state = robot_state  # Состояние робота (координаты и ориентация)
        self.ax = ax  # Объект matplotlib для отображения графики

        self.robot_marker=ax.add_trace(go.Scatter(x=[robot_state.x],y=[robot_state.y],marker=dict(
            size=20,  # Размер точек
            color='rgb(255, 0, 0)',  # Цвет точек
            symbol='triangle-up',  # Использование треугольных маркеров
            line=dict(
                width=2,  # Ширина обводки точек
                color='rgb(0, 0, 0)'  # Цвет обводки точек
            ))))# Маркер для позиции робота
        
        self.light_indicator = None  # Индикатор света
        self.lights_on = False  # Состояние света
        self.path_line = ax.add_trace(go.Scatter(x=[],y=[]))  # Линия для отображения пути
        self.graph = Math.build_graph(self=self)  # Построение графа на основе карты маркеров
        self.closestmarkers = Math.find_closest_marker_id(self=self)
        self.plot_map()  # Отображение карты маркеров на графике

Данная функция является конструктором для класса SimulationController. В данном классе конструктор инициализирует настройки системы ровера, использующего маркеры ArUco для навигации.

Описание функции init Параметры: aruco_map (dict): Словарь, содержащий координаты маркеров ArUco. Эти маркеры используются для определения местоположения и ориентации ровера в пространстве. robot_state (ContactState): Объект, представляющий состояние робота, включая его координаты (x, y) и ориентацию. Это позволяет отслеживать текущее местоположение и направление движения ровера. ax (matplotlib.axes.Axes): Объект из библиотеки Matplotlib, используемый для отображения графики. Он определяет область для рисования на графике. HOST_SOCKET (str): Адрес хоста для подключения по сокету (по умолчанию 'localhost'). PORT_SOCKET (int): Порт для подключения по сокету (по умолчанию 5530).

Функциональность: Создание сокета: Конструктор создает TCP-сокет для связи с сервером, что позволяет роботу обмениваться данными или получать команды. Инициализация атрибутов: Сохраняются параметры aruco_map, robot_state, и ax, чтобы их можно было использовать в других методах класса. Отображение робота на графике: Добавляется маркер для позиции робота на графике с заданным размером, цветом и формой (треугольник). Инициализация дополнительных параметров: Создаются переменные для индикатора света и линии пути, что позволяет отслеживать состояние освещения и движение робота соответственно. Построение графа: Вызывается метод для построения графа на основе карты маркеров, что позволяет анализировать связи между маркерами. Поиск ближайших маркеров: Метод используется для нахождения ближайших к роботу маркеров, что может быть полезно для навигации и принятия решений. Отображение карты: Вызывается функция для отображения карты маркеров на графике, что позволяет визуализировать положение робота относительно этих маркеров.

ФУНКЦИЯ UPDATE_SITE

def update_site(self):
        """
        Отображение графика на сайте
        
        Описание:
        Отправляет график на сайт в ввиде json строки
        """
        fig_json = json.dumps(self.ax, cls=PlotlyJSONEncoder)
        self.client_socket.sendall(f"{fig_json}".encode('utf-8'))

Функция update_site отвечает за отображение графика на сайте, что позволяет визуализировать данные и состояние ровера для пользователей. Она отправляет график в формате JSON через сокет на веб-сервер.

ПРИНЦИП РАБОТЫ:

Генерация JSON-строки:

Функция начинает с преобразования графика (представленного атрибутом self.ax) в формат JSON. Для этого используется json.dumps, что позволяет превратить объект графика в строку, которую можно отправить по сети.

Используется класс PlotlyJSONEncoder, чтобы гарантировать корректное кодирование объектов, специфичных для библиотеки Plotly.

Отправка данных:

Полученная JSON-строка сохраняется в переменной fig_json.

Затем функция использует метод sendall сокета (self.client_socket) для отправки этой строки на сервер. Строка предварительно кодируется в байты с помощью .encode('utf-8'), что необходимо для передачи данных по сети.

Таким образом, update_site играет ключевую роль в поддержании связи между ровером и пользователем через визуализацию данных на веб-сайте.

ФУНКЦИЯ PLOT_MAP

def plot_map(self):
        """
        Отображение карты маркеров на графике.

        Описание:
        Рисует маркеры ArUco и их идентификаторы на графике. Также настраивает оси и добавляет сетку и легенду.
        """
        for marker_id, marker_info in self.aruco_map.items():
            self.ax.add_trace(go.Scatter(x=[marker_info['x']], y=[marker_info['y']]))
            self.ax.add_annotation(
                x=marker_info['x'],
                y=marker_info['y'],
                text=f'{marker_id}',
                showarrow=False,
                yshift=-10
            )

Функция plot_map предназначена для визуализации карты маркеров ArUco. Она рисует маркеры и их идентификаторы на графике, а также настраивает оси, добавляет сетку и легенду, чтобы пользователи могли легко интерпретировать данные.

ПРИНЦИП РАБОТЫ:

Итерация по маркерам:

Функция начинает с цикла по всем маркерам в self.aruco_map. Каждый маркер представлен в виде словаря, где ключом является его уникальный идентификатор (marker_id), а значением — информация о маркере (marker_info).

Добавление маркеров на график:

Для каждого маркера создается новый след (trace) на графике с использованием метода add_trace из библиотеки Plotly.

В координатах x и y указываются значения из словаря marker_info, которые обозначают положение конкретного маркера

Добавление аннотации:

После добавления маркера, функция создает аннотацию для каждого маркера, используя метод add_annotation.

Аннотация содержит текст с идентификатором маркера (f'{marker_id}') и размещается непосредственно над точкой маркера. Параметр yshift=-10 используется для смещения текста вниз, чтобы избежать наложения на сам маркер.

Настройка графика (не показано в коде):

Хотя в приведенном коде не указаны настройки осей, сетки или легенды, обычно в подобных функциях эти элементы тоже настраиваются для лучшего восприятия данных. Это может включать установку имен осей, границ, добавление сетки и легенд для пояснения к графику.

ФУНКЦИЯ UPDATE_ROBOT_POSITION

def update_robot_position(self, x, y, orientation):
        """
        Обновление позиции робота на графике.

        Параметры:
        - x (float): Новая координата X робота.
        - y (float): Новая координата Y робота.
        - orientation (float): Новая ориентация робота.

        Описание:
        Обновляет координаты и ориентацию робота, а также перерисовывает маркер и линию пути на графике.
        """
        self.robot_state.update_position(x, y)
        self.robot_state.update_orientation(orientation)
        self.robot_marker.data[0].x=[x]
        self.robot_marker.data[0].y=[y]
        if self.lights_on:
            if self.light_indicator:
                self.light_indicator.layout.annotations = self.light_indicator.layout.annotations[:-1]
            self.light_indicator = self.ax.add_annotation(x=x, y=y, text="Lights On", 
                                    showarrow=True, arrowhead=1, arrowsize=2, arrowwidth=2,
                                    arrowcolor='yellow')
        self.update_site()
        plt.pause(0.5)

Функция update_robot_position предназначена для обновления текущего положения и ориентации ровера в двумерном пространстве. Она также перерисовывает маркер, представляющий ровера, и (возможно) добавляет визуальный индикатор света на графике.

ПРИНЦИП РАБОТЫ:

Обновление состояния робота:

Сначала функция обновляет координаты ровера, вызывая метод update_position класса, к которому относится объект self.robot_state. Это позволяет хранить актуальные данные о текущем положении ровера.

Обновление ориентации:

Затем обновляется ориентация ровера с помощью метода update_orientation, передавая новый угол как параметр.

Перерисовка маркера робота:

Следующий шаг заключается в обновлении данных о маркере, который представляет ровера на графике. Координаты x и y маркера заменяются на новые значения.

Управление индикатором света:

Если свет включен (self.lights_on), функция проверяет, существует ли уже индикатор света (self.light_indicator). Если он есть, он удаляется из списка аннотаций графика, чтобы избежать наложения.

Затем добавляется новая аннотация, указывающая, что свет включен ("Lights On"), с различными параметрами оформления стрелки:

Обновление других элементов графика:

Метод self.update_site() может обновлять другие элементы графика, такие как линии пути или другие визуальные представления, связанные с движением ровера.

Пауза для обновления графика:

Наконец, вызов plt.pause(0.5) ставит график на паузу на полсекунды, позволяя пользователю увидеть изменения, прежде чем они будут обновлены снова.

ФУНКЦИЯ PLOT_ORIENTATION_LINE

def plot_orientation_line(self, x, y, orientation):
        """
        Отображение линии ориентации робота на графике.

        Параметры:
        - x (float): Координата X начальной точки линии ориентации.
        - y (float): Координата Y начальной точки линии ориентации.
        - orientation (float): Ориентация робота.

        Описание:
        Рисует линию, показывающую направление ориентации робота, исходя из текущей позиции и ориентации.
        """
        length = 0.1  # Длина линии ориентации
        end_x = x + length * np.cos(np.radians(orientation))
        end_y = y + length * np.sin(np.radians(orientation))
        self.ax.plot([x, end_x], [y, end_y], 'r-', label='Orientation')  # Отрисовка линии ориентации

Функция plot_orientation_line предназначена для визуализации направления, в котором ориентирован ровер. Она рисует линию, исходя из текущей позиции и угла ориентации робота.

ПРИНЦИП РАБОТЫ:

Определение длины линии:

Переменная length задает длину линии ориентации, которая будет отрисована. В данном случае она равна 0.1, что означает, что линия будет длиной 10% от единичного расстояния.

Вычисление конечных координат линии:

Конечные координаты end_x и end_y вычисляются с использованием тригонометрических функций. Угол ориентации преобразуется из градусов в радианы с помощью функции np.radians(), так как тригонометрические функции в NumPy работают с радианами.

Отрисовка линии ориентации:

Метод self.ax.plot() используется для рисования линии на графике. Линия рисуется от начальной точки (x, y) до конечной точки (end_x, end_y).

Параметр 'r-' указывает, что линия будет красного цвета ('r') и сплошной ('-').

Аргумент label='Orientation' позволяет задать метку для этой линии, что может быть полезно для создания легенды графика.

ФУНКЦИЯ PERFORM_MOVEMENT

async def perform_movement(self, distance_cm):
        """
        Выполнение движения робота на заданное расстояние.

        Параметры:
        - distance_cm (float): Расстояние для перемещения в сантиметрах.

        Описание:
        Вычисляет новые координаты робота на основе текущей позиции и ориентации, затем обновляет состояние робота
        и перерисовывает его на графике. Имитирует время движения с помощью asyncio.sleep.
        """
        distance_m = distance_cm / 100
        rad = np.radians(self.robot_state.orientation)
        new_x = self.robot_state.x + distance_m * np.cos(rad)
        new_y = self.robot_state.y + distance_m * np.sin(rad)
        x_values = [float(self.robot_marker.data[0].x[0]), new_x]
        y_values = [float(self.robot_marker.data[0].y[0]), new_y]
        self.robot_state.x, self.robot_state.y = new_x, new_y
        self.ax.add_trace(go.Scatter(
            x=x_values,
            y=y_values,
            mode='lines',
            line=dict(color='red')  # Указываем цвет линии
        ))
        self.robot_marker.data[0].x=[new_x]
        self.robot_marker.data[0].y=[new_y]
        if self.lights_on:
            if self.light_indicator:
                self.light_indicator.layout.annotations = self.light_indicator.layout.annotations[:-1]
            self.light_indicator = self.ax.add_annotation(x=new_x, y=new_y, text="Lights On", 
                                    showarrow=True, arrowhead=1, arrowsize=2, arrowwidth=2,
                                    arrowcolor='yellow')
            
        logging.info(f"Moving: {distance_cm}cm forward.")
        self.update_site()
        await asyncio.sleep(0.5)  # Simulate time it takes to move

Функция perform_movement управляет передвижением ровера по заданному расстоянию в сантиметрах. Она обновляет состояние ровера, вычисляет новые координаты, а также отображает изменения на графике. Время движения имитируется с помощью asyncio.sleep.

ПРИНЦИП РАБОТЫ:

Преобразование расстояния в метры:

Переменная distance_m хранит преобразованное значение расстояния в метрах. Это необходимо, поскольку дальнейшие вычисления основываются на метрах.

Вычисление новых координат:

Угол ориентации ровера преобразуется из градусов в радианы с помощью np.radians().

Новые координаты new_x и new_y рассчитываются с учетом текущих координат (self.robot_state.x и self.robot_state.y) и направления движения, используя тригонометрические функции np.cos() и np.sin().

Обновление состояния робота:

Создаются списки x_values и y_values, содержащие старые и новые координаты ровера для дальнейшей отрисовки.

Обновляются значения координат ровера в состоянии self.robot_state.

Отрисовка линии движения:

На графике добавляется линия, показывающая путь, который прошел ровера. Используется библиотека Plotly для создания линейной графики (модуль go).

Обновление маркера робота:

Маркер робота на графике обновляется до новых координат.

Индикация освещения:

Если освещение включено (self.lights_on), проверяется, есть ли предыдущая аннотация индикатора света. Если да, она удаляется.

Затем добавляется новая аннотация на график, указывающая, что огни включены, с соответствующими параметрами визуализации.

Логирование действия:

Записывается лог (logging.info) о том, что ровер перемещается на указанное расстояние.

Обновление сайта и ожидание:

Вызывается метод self.update_site() для обновления состояния или интерфейса.

С помощью await asyncio.sleep(0.5) эмулируется время, необходимое для выполнения движения. Это позволяет функции работать асинхронно, не блокируя другие операции.

ФУНКЦИЯ PERFORM_SPIN_ANGLE

async def perform_spin_angle(self, angle):
        """
        Выполнение поворота робота на заданный угол.

        Параметры:
        - angle (float): Угол поворота в градусах.

        Описание:
        Вычисляет новую ориентацию робота и обновляет его состояние. Затем перерисовывает робота на графике.
        Имитация времени поворота осуществляется с помощью asyncio.sleep.
        """
        current_orientation = self.robot_state.orientation % 360
        new_orientation = (current_orientation + angle) % 360
        angle_difference = (new_orientation - current_orientation + 360) % 360
        if angle_difference > 180:
            angle_difference -= 360  # Наименьший угол поворота
        
        self.robot_state.orientation = new_orientation
        self.update_robot_position(self.robot_state.x, self.robot_state.y, new_orientation)
        logging.info(f"Spinning: {angle_difference} degrees {'clockwise' if angle_difference >= 0 else 'counterclockwise'}.")
        await asyncio.sleep(0.5)  # Имитация времени, необходимого для поворота

Функция perform_spin_angle предназначена для того, чтобы управлять поворотом ровера на заданный угол, обновляя его ориентацию и визуальное представление.

ПРИНЦИП РАБОТЫ:

Вычисления текущей и новой ориентации

Текущая ориентация:

Получается текущее значение ориентации ровера из состояния (self.robot_state.orientation) и используется оператор остатка % для ограничения значения в пределах от 0 до 359 градусов.

Новая ориентация:

Новая ориентация (new_orientation) вычисляется путём сложения текущей ориентации и угла поворота, также с применением оператора остатка для приведения к диапазону от 0 до 359 градусов.

Вычисление разницы углов

Разница углов:

Сначала вычисляется разница (angle_difference) между новой и текущей ориентацией.

Этот расчёт также включает добавление 360 для корректной обработки отрицательной разницы.

Минимизация угла поворота:

Если разница превышает 180 градусов, то вычитается 360. Это помогает определить наименьший угол поворота, что может быть полезно для управления движением ровера.

Обновление состояния робота

Обновляется ориентация робота в состоянии (robot_state), присваивая новое значение, рассчитанное ранее.

Обновление визуального представления

Метод update_robot_position вызывается для обновления положения ровера на графике. Используются текущие координаты x и y, а также новая ориентация.

Логирование информации о повороте

Записывается информация о выполненном повороте в лог (logging.info). Указываются величина поворота и направление (по часовой стрелке или против).

Имитация времени поворота

С помощью await asyncio.sleep(0.5) происходит приостановка выполнения функции на 0.5 секунды, имитируя время, необходимое для выполнения физического поворота ровера. Это позволяет остальным асинхронным задачам продолжать выполняться без блокировки.

ФУНКЦИЯ CONTROL_LIGHTS

async def control_lights(self, on: bool):
        """
        Асинхронная функция для включения/выключения света на роботе (визуализация).

        Параметры:
        - on (bool): Флаг включения/выключения света.

        Описание:
        Визуализирует включение или выключение света на графике путем изменения цвета или добавления аннотации.
        """
        self.lights_on = on
        if on:
            if self.light_indicator:
                self.light_indicator.layout.annotations = self.light_indicator.layout.annotations[:-1]
            self.light_indicator = self.ax.add_annotation(x=self.robot_marker.data[0].x, y=self.robot_marker.data[0].y, text="Lights On", 
                                    showarrow=True, arrowhead=1, arrowsize=2, arrowwidth=2,
                                    arrowcolor='yellow')
        else:
            if self.light_indicator:
                self.light_indicator.layout.annotations = self.light_indicator.layout.annotations[:-1]
        
        action = "включены" if on else "выключены"
        message = f"Свет {action}."
        self.update_site()
        logging.info(message)

Эта асинхронная функция отвечает за управление светом на ровере. Она принимает один параметр и выполняет несколько действий для обновления состояния света и его визуализации.

Установка состояния света

Здесь мы обновляем состояние света ровера (lights_on), присваивая ему значение on. Если on равно True, значит свет включен; если False, свет выключен.

Включение света

Проверяем, нужно ли включить свет (if on:).

Если свет уже включен (то есть существует индикатор света), мы убираем последний элемент из списка аннотаций (это необходимо для корректного обновления визуализации).

Затем добавляем новую аннотацию на график, чтобы визуально показать, что свет включен.

Указываем координаты x и y, где будет располагаться аннотация, а также задаем текст "Lights On".

Параметры showarrow, arrowhead, arrowsize, arrowwidth, и arrowcolor позволяют настроить внешний вид стрелки, указывающей на позицию света.

Выключение света

Если свет выключается (ветвь else), мы проверяем, существует ли индикатор.

Если он существует, удаляем последнюю аннотацию с графика, тем самым визуально указывая, что свет теперь выключен.

Формирование сообщения

Здесь мы формируем строку для логирования (action): если свет включен, то сообщение будет "Свет включены.", иначе — "Свет выключены."

Обновление состояния сайта

self.update_site()

Эта строка вызывает метод для обновления сайта или интерфейса пользователя. Это важно, чтобы изменения стали видимыми для пользователя.

Логирование информации

В конце функции записываем сформированное сообщение в лог (logging.info). Это полезно для отслеживания состояния системы и ее работы.

ФУНКЦИЯ NAVIGATE_TO_MARKER

async def navigate_to_marker(self, goal_id):
        """
        Навигация к указанному маркеру.

        Параметры:
        - goal_id (int): Идентификатор целевого маркера.

        Описание:
        Использует алгоритм A* для нахождения пути к целевому маркеру и выполняет движение по этому пути.
        Если путь не найден, выводится сообщение об ошибке.
        """
        path = await self.astar_algorithm(self.find_closest_marker_id(), goal_id)
        if not path:
            logging.info("No path found.")
            return

        logging.info(f"Path found: {path}")
        for marker_id in path:
            await self.move_to_marker(marker_id)
            self.update_site()

Эта асинхронная функция управляет движением ровера к целевому маркеру, используя заданный идентификатор (goal_id).

ПРИНЦИП РАБОТЫ:

Нахождение пути с использованием алгоритма A*

Здесь вызывается метод astar_algorithm, который реализует алгоритм A* для нахождения пути.

Метод принимает два параметра: идентификатор ближайшего маркера, получаемый через self.find_closest_marker_id(), и идентификатор целевого маркера (goal_id).

Использование await позволяет дождаться завершения работы алгоритма перед продолжением выполнения функции.

Проверка наличия пути

После выполнения алгоритма мы проверяем, был ли найден путь. Если переменная path пустая или равна None, это означает, что путь не найден.

В таком случае записываем сообщение в лог (logging.info) и завершаем выполнение функции с помощью оператора return.

Логирование найденного пути

Если путь был найден, записываем его в лог (logging.info) для дальнейшего анализа или отладки. Это может помочь разработчикам понять, какой маршрут выбрал ровер.

Перемещение по каждому маркеру на пути

Используя цикл for, функция проходит по каждому идентификатору маркера в списке path.

Для каждого маркера вызывается метод move_to_marker(marker_id), который отвечает за перемещение ровера к данному маркеру. Опять же, используется await для ожидания завершения этого процесса.

После перемещения обновляется интерфейс или сайт, чтобы отображались изменения, вызванные перемещением ровера.

ФУНКЦИЯ MOVE_TO_MARKER

async def move_to_marker(self, marker_id):
        """
        Движение к указанному маркеру.

        Параметры:
        - marker_id (int): Идентификатор маркера для движения.

        Описание:
        Рассчитывает угол поворота и расстояние до маркера, выполняет поворот и движение к маркеру.
        Обновляет текущее состояние робота после выполнения движения.
        """
        target = self.aruco_map[marker_id]
        dx = target['x'] - self.robot_state.x
        dy = target['y'] - self.robot_state.y
        target_angle = math.degrees(math.atan2(dy, dx))
        await self.perform_spin_angle(target_angle - self.robot_state.orientation)
        distance = math.sqrt(dx ** 2 + dy ** 2) * 100
        await self.perform_movement(distance)

Эта асинхронная функция управляет движением ровера к целевому маркеру, используя его идентификатор (marker_id).

ПРИНЦИП РАБОТЫ:

Получение координат маркера

Здесь мы извлекаем информацию о целевом маркере из словаря aruco_map по его идентификатору marker_id. Объект target теперь содержит координаты этого маркера (например, x и y).

Расчет изменений координат

Переменные dx и dy представляют собой изменения в координатах между текущим состоянием робота и координатами целевого маркера:

dx — изменение по оси X.

dy — изменение по оси Y.

Расчет угла поворота

Здесь мы используем функцию atan2 для вычисления угла к маркеру, исходя из координат dx и dy.

Результат переводится из радиан в градусы с помощью math.degrees, чтобы получить нужный угол для поворота.

Выполнение поворота

Перед тем как двигаться к маркеру, ровер должен повернуться в нужном направлении.

Метод perform_spin_angle вызывается с аргументом, представляющим разницу между целевым углом (target_angle) и текущей ориентацией ровера (self.robot_state.orientation). Это позволяет роверу правильно настроить своё направление перед перемещением.

Расчет расстояния до маркера

После поворота рассчитываем расстояние до маркера (distance) с использованием теоремы Пифагора.

Значение умножается на 100, вероятно, для перевода в соответствующую единицу измерения (например, сантиметры или миллиметры).

Выполнение движения к маркеру

Теперь, когда робот развернулся в нужную сторону, он двигается к маркеру, вызывая метод perform_movement с расстоянием (distance), рассчитанным ранее.

ФУНКЦИЯ MOVE_TO_POSITION

def move_to_position(self, x, y):
        """
        Движение к указанной позиции.

        Параметры:
        - x (float): Координата X целевой позиции.
        - y (float): Координата Y целевой позиции.

        Описание:
        Рассчитывает угол поворота и расстояние до целевой позиции, затем выполняет поворот и движение.
        """
        dx = x - self.robot_state.x
        dy = y = self.robot_state.y
        target_heading = math.degrees(math.atan2(dy, dx))
        distance = math.sqrt(dx ** 2 + dy ** 2) * 100

        self.perform_spin_angle((target_heading - self.robot_state.orientation) % 360 - 180, clockwise=True)
        self.perform_movement(distance)

Эта функция управляет движением ровера к указанной цели, определяемой координатами x и y.

ПРИНЦИП РАБОТЫ:

Расчет изменений координат

Здесь мы вычисляем изменения координат:

dx — разница между целевой координатой X (x) и текущей координатой X ровера (self.robot_state.x).
dy — разница между целевой координатой Y (y) и текущей координатой Y ровера (self.robot_state.y)..

Расчет угла поворота

Угол к целевой позиции (target_heading) вычисляется с использованием функции atan2, которая возвращает угол в радианах.

Этот угол затем переводится в градусы с помощью math.degrees.

Расчет расстояния до позиции

Мы используем теорему Пифагора для вычисления расстояния до целевой позиции (distance):

distance рассчитывается как квадратный корень из суммы квадратов dx и dy.

Результат умножается на 100 для перевода в нужные единицы измерения (например, сантиметры или миллиметры).

Выполнение поворота

Теперь необходимо повернуть ровера в сторону целевой позиции.

Вычисляется угол поворота как разница между целевым углом (target_heading) и текущим углом ориентации ровера (self.robot_state.orientation).

Используется операция % 360, чтобы гарантировать, что угол входит в диапазон от 0 до 360 градусов.

Затем вычитается 180, чтобы правильно настроить угол поворота.

Метод perform_spin_angle вызывается с указанием направления поворота (clockwise=True), чтобы робот вращался по часовой стрелке.

Выполнение движения к позиции

После того как ровер повернулся, он готов двигаться к целевой позиции.

Метод perform_movement вызывается с аргументом distance, который представляет собой расстояние до цели.

ФУНКЦИЯ SHOW

def show(self):
        """
        Отображение графического интерфейса.

        Описание:
        Вызывает метод show библиотеки matplotlib для отображения графического интерфейса.
        """
        plt.show()

Отображение графического интерфейса

Здесь вызывается метод show из Matplotlib (plt — это общепринятое имя для модуля matplotlib.pyplot).

Этот метод запускает цикл событий и отображает все текущие графики или фигуры, которые были подготовлены к показу.

Если графиков нет, окно будет пустым, но оно все равно будет открыто.

КЛАСС ROVER_CONTROLLER

Класс для управления ровером через сервер с использованием Socket.IO

ПРИМЕЧАНИЕ: В данном блоке могут отсутствовать некоторые объяснения ввиду схожести функций.

Большая часть функционала дублируется из симулятора на реальном ровере

ФУНКЦИЯ INIZIALIZE_SOCKETIO


async def initialize_socketio(self):
        """
        Асинхронная функция для инициализации и подключения клиента Socket.IO к серверу.

        Описание:
        Подключается к серверу с использованием URL и токена авторизации. Если подключение успешно, 
        выводится сообщение об успешном подключении. В случае ошибки подключения, выводится сообщение об ошибке.
        """
        # try:
        await self.sio.connect(self.server_url, auth={"token": self.token}, namespaces=['/vehicles'])
        logging.info("Connected to Socket.IO server")
        # except socketio.exceptions.ConnectionError as e:
        #     logging.error(f"Failed to connect to server: {e}")

Эта функция выполняет следующие задачи:

Подключает клиент Socket.IO к указанному серверу.
Обрабатывает успешное подключение и ошибки при подключении.

Подключение к серверу

Здесь вызывается метод connect у объекта self.sio (предположительно, это экземпляр клиента Socket.IO).

Используются три аргумента:

self.server_url: URL сервера для подключения.

auth={"token": self.token}: передача токена авторизации для аутентификации на сервере.

namespaces=['/vehicles']: указание namespace, с которым будет работать клиент.

await позволяет дождаться завершения операции подключения перед продолжением выполнения кода.

Логирование успешного подключения

Если подключение прошло успешно, выводится информационное сообщение в лог (LOGGING.INFO) о том, что клиент успешно подключился к серверу.

ФУНКЦИЯ WRITE_REPORT

async def write_report(message):
        """
        Асинхронная функция для записи сообщений в файл отчета.

        Параметры:
        - message (str): Сообщение для записи в отчет.

        Описание:
        Создает файл отчета и записывает в него сообщения, используемые для ведения журнала действий робота.
        """
        filename = f'Отчет_Фамилия_Имя.txt'
        async with aiofiles.open(filename, mode='a', encoding='utf-8') as file:
            await file.write(message + "\n")

Асинхронная функция для записи сообщений в файл отчета. Если точнее, создает файл отчета и записывает в него сообщения, используемые для ведения журнала действий робота.

ФУНКЦИЯ ON_MOVE

async def on_move(self, result):
        """
        Колбэк для обработки ответов сервера на команды движения.

        Параметры:
        - result (dict): Результат выполнения команды движения.

        Описание:
        Записывает результаты выполнения команд движения в отчет.
        """
        await RoverController.write_report(f'Command done: {result}')

Функция записывает результаты выполнения команд движения в отчет.

Процесс подготовки кода:

Создайте файл Contact_controller.py:

Поэтапно копируйте код в файл, в итоге окно программирования должно выглядеть так

Установите необходимые библиотеки: в данном случае у нас не установлены следующие библиотеки: matplotlib.pyplot, numpy, aiofiles, plotly. Если они у вас не установлены, то вот как их установить:
1. Откройте терминал в Visual Studio Code (Terminal - New Terminal или комбинацией Ctrl + Shift + `)
2. Устанавливаем библиотеки: поочередно в консоль прописываем следующие библиотеки, запускать каждую команду по отдельности: pip install matplotlib; pip install numpy; pip install aiofiles; pip install plotly
Так же у нас имеются две библиотеки, которые установить не получится. Это всё из-за того, что мы переносим в этот файл данные из других файлов, в данном случае это Math и Frenet_frame. Они у вас заработают как только в иерархию добавятся коды с такими названиями!

При запуске кода в консоль выводится следующее: [WinError 10061] Подключение не установлено, т.к. конечный компьютер отверг запрос на подключение. Связано это с тем, что команды на движение передаются в файл Main.py, оттуда всё будет работать как надо.

PreviousФАЙЛ «ARUCO.PY»NextФАЙЛ «CONTACT_STATE.PY»

Last updated 7 days ago