From 0d5ef2ec4ae1bdc7b3f9927a7c3cbe649c1bf998 Mon Sep 17 00:00:00 2001 From: Anton Pisarenko Date: Tue, 3 Sep 2024 15:34:59 +0300 Subject: [PATCH] Run drone with control --- .devcontainer/devcontainer.json | 124 ++++++++++++++++++++++---------- CMakeLists.txt | 4 +- docker/Dockerfile.ros_px4 | 2 + docker/docker-compose.yml | 4 +- docs/commands.md | 7 +- docs/control.md | 67 +++++++++++++++++ launch/aerial_drone_base.py | 39 +++++----- scripts/control.py | 21 ++++-- scripts/processes.py | 116 ++++++++++++++++++++++-------- scripts/velocity_control.py | 116 ++++++++++++++++-------------- scripts/visualizer.py | 3 +- 11 files changed, 351 insertions(+), 152 deletions(-) create mode 100644 docs/control.md diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 1e0e2cd..0003905 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,41 +1,87 @@ { - "workspaceFolder": "/workspace", - "name": "Ubuntu", - - "settings": { - "terminal.integrated.shell.linux": "/bin/bash", - "editor.formatOnSave": true, - "python.languageServer": "Pylance", - "python.pythonPath": "/usr/bin/python3", - "python.autoComplete.extraPaths": [ - "/opt/ros/noetic/lib/python3/site-packages", - "/opt/ros/noetic/local/lib/python3/dist-packages", - ], - "python.analysis.extraPaths": [ - "/opt/ros/noetic/lib/python3/site-packages", - "/opt/ros/noetic/local/lib/python3/dist-packages", - ], - }, + "name": "aerial_drone_base Development Container", + // "image": "ros:humble", // Используем Docker-образ с ROS 2 Humble - "extensions": [ - "christian-kohler.path-intellisense", - "codezombiech.gitignore", - "donjayamanne.python-environment-manager", - "donjayamanne.python-extension-pack", - "eamodio.gitlens", - "esbenp.prettier-vscode", - "KevinRose.vsc-python-indent", - "konicy.conan-extension", - "ms-python.python", - "ms-python.vscode-pylance", - "ms-vscode.cmake-tools", - "ms-vscode.cpptools", - "ms-vscode.cpptools-extension-pack", - "ms-vscode.cpptools-themes", - "redhat.vscode-xml", - "redhat.vscode-yaml", - "twxs.cmake", - "VisualStudioExptTeam.intellicode-api-usage-examples", - "VisualStudioExptTeam.vscodeintellicode", - ], -} \ No newline at end of file + // Рабочая директория внутри контейнера + "workspaceFolder": "/workspace", + + "settings": { + "terminal.integrated.shell.linux": "/bin/bash", // Bash как основная оболочка + "editor.formatOnSave": true, // Автоформатирование при сохранении + "python.languageServer": "Pylance", // Использование Pylance для Python + "python.pythonPath": "/usr/bin/python3", // Путь к Python 3 + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3/site-packages", // Путь для ROS 2 Humble + "/opt/ros/humble/local/lib/python3/dist-packages", + "/workspace/install/px4/local/lib/python3.10/dist-packages", + "/workspace/install/px4_msgs/local/lib/python3.10/dist-packages", + "/workspace/install/px4_ros_com/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3/site-packages", // Путь для ROS 2 Humble + "/opt/ros/humble/local/lib/python3/dist-packages", + "/workspace/install/px4/local/lib/python3.10/dist-packages", + "/workspace/install/px4_msgs/local/lib/python3.10/dist-packages", + "/workspace/install/px4_ros_com/local/lib/python3.10/dist-packages" + ], + "files.associations": { + "*.launch": "xml", // Ассоциация файлов .launch с XML + "*.launch.py": "python" // Ассоциация файлов .launch.py с Python + }, + "cmake.configureOnOpen": true, // Автоматическая конфигурация CMake при открытии + "ros.distro": "humble", // Установка ROS 2 дистрибутива + + "python.analysis.exclude": [ + "**/__pycache__", + "**/site-packages", + "**/build", + "**/install", + "**/devel", + "**/log", + "**/.venv", + "**/env" + ], + // "files.exclude": { + // "**/__pycache__": true, + // "**/.git": true, + // "**/.vscode": true, + // "**/build": true, + // "**/install": true, + // "**/devel": true, + // "**/log": true, + // "**/env": true, + // "**/.venv": true + // }, + // "python.analysis.autoSearchPaths": false, + // "python.analysis.useLibraryCodeForTypes": false + }, + + "extensions": [ + "ms-python.python", // Расширение Python + "ms-python.vscode-pylance", // Pylance для Python + "ms-vscode.cpptools", // Поддержка C++ + "ms-vscode.cmake-tools", // Инструменты CMake + "twxs.cmake", // CMake язык + "esbenp.prettier-vscode", // Prettier для форматирования кода + "ms-iot.vscode-ros", // Поддержка ROS в VS Code + "redhat.vscode-xml", // Поддержка XML (для .launch файлов) + "redhat.vscode-yaml", // Поддержка YAML (для конфигурационных файлов) + "donjayamanne.python-environment-manager", // Управление Python окружениями + "KevinRose.vsc-python-indent", // Улучшенная поддержка отступов для Python + "VisualStudioExptTeam.vscodeintellicode", // IntelliCode для улучшенной подсказки кода + "VisualStudioExptTeam.intellicode-api-usage-examples", // Примеры использования API от IntelliCode + "ms-vscode.cpptools-themes", // Темы для C++ + "codezombiech.gitignore", // Поддержка .gitignore + "christian-kohler.path-intellisense" // Подсказки для путей + ], + + // "postCreateCommand": "source /opt/ros/humble/setup.bash && colcon build", // Команда для настройки ROS 2 и сборки проекта + + "remoteEnv": { + "ROS_DISTRO": "humble" // Переменная окружения для ROS 2 + }, + + // "forwardPorts": [11311, 8080], // Проброс портов, например, для ROS master и web-интерфейсов + + // "shutdownAction": "stopContainer" // Остановка контейнера при закрытии VS Code +} diff --git a/CMakeLists.txt b/CMakeLists.txt index 19c50e1..220f1a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,13 +15,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(urdf REQUIRED) -find_package(xacro REQUIRED) +# find_package(xacro REQUIRED) find_package(robot_state_publisher REQUIRED) find_package(rviz2 REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(px4_msgs REQUIRED) +# find_package(px4_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) diff --git a/docker/Dockerfile.ros_px4 b/docker/Dockerfile.ros_px4 index 5f7f459..ed604d9 100644 --- a/docker/Dockerfile.ros_px4 +++ b/docker/Dockerfile.ros_px4 @@ -37,6 +37,8 @@ RUN apt-get update && apt-get install -y \ git \ tmux \ tree \ + gnome-terminal \ + dbus-x11 \ && rm -rf /var/lib/apt/lists/* # Добавляем репозиторий ROS 2 и устанавливаем его diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 5c91a08..ec3b4db 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -2,8 +2,8 @@ name: aerial_drone services: ros-humble-master: - # mem_limit: 4G # выделение до 4 гигабайт памяти - # cpus: 2.0 # ограничение до двух процессорных ядер + mem_limit: 4G # выделение до 4 гигабайт памяти + cpus: 2.0 # ограничение до двух процессорных ядер build: context: ../ dockerfile: ./docker/Dockerfile.ros_px4 diff --git a/docs/commands.md b/docs/commands.md index bd8c221..e2a30da 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -23,7 +23,7 @@ MicroXRCEAgent udp4 -p 8888 reboot now ros2 topic echo /fmu/out/vehicle_status -``` + ENV PX4_HOME_LAT=50 ENV PX4_HOME_LON=23 @@ -31,4 +31,7 @@ ENV PX4_HOME_ALT=100 docker inspect -f ros_2_humle-ros-humble-master-1 -export PX4_SIM_HOSTNAME=172.26.176.1 \ No newline at end of file +export PX4_SIM_HOSTNAME=172.26.176.1 + + +``` \ No newline at end of file diff --git a/docs/control.md b/docs/control.md new file mode 100644 index 0000000..ec8d09b --- /dev/null +++ b/docs/control.md @@ -0,0 +1,67 @@ +### Общий ход выполнения + +1. **Инициализация узла ROS 2**: + - Код начинается с инициализации ROS 2 и создания узла `OffboardControl` в функции `main()`. + - После создания узла запускается цикл обработки событий ROS 2 с помощью `rclpy.spin(offboard_control)`, который позволяет узлу работать в фоновом режиме и обрабатывать входящие сообщения и таймеры. + +2. **Создание подписок и издателей**: + - В конструкторе класса `OffboardControl` создаются подписки на различные топики (например, `/fmu/out/vehicle_status`, `/offboard_velocity_cmd`) и издатели для отправки сообщений на топики, связанные с управлением дроном. + - Также создаются таймеры для периодической проверки и отправки команд на армирование (`arm_timer_callback`) и основного цикла управления (`cmdloop_callback`). + +### Основные методы и их вызов + +1. **Метод `arm_timer_callback`**: + - Этот метод вызывается каждые 0.1 секунды (период `arm_timer_period`). Он выполняет функции управления состоянием дрона (Finite State Machine, FSM). + - В зависимости от текущего состояния дрона (`self.current_state`), метод решает, что делать дальше: + - В состоянии `IDLE`, если дрон прошел проверку перед полетом (`self.flightCheck == True`) и была получена команда на армирование (`self.arm_message == True`), дрон переходит в состояние `ARMING`. + - В состоянии `ARMING`, если дрон успешно армирован, он переходит в состояние `TAKEOFF`. + - В состоянии `TAKEOFF`, если дрон начал взлет (состояние NAV_STATE_AUTO_TAKEOFF), он переходит в состояние `LOITER`. + - В состоянии `LOITER`, если дрон достиг состояния зависания (LOITER), он переходит в режим `OFFBOARD`. + - В состоянии `OFFBOARD`, если дрон все еще армирован и нет аварийной ситуации (failsafe), продолжается отправка команд управления дроном. + - Этот метод управляет основными переходами между состояниями и инициирует отправку команд армирования, взлета и перехода в режим `Offboard`. + +2. **Метод `cmdloop_callback`**: + - Этот метод вызывается каждые 0.02 секунды (период `timer_period`). Он отвечает за отправку команд управления движением дрона, когда дрон находится в режиме `Offboard`. + - Метод проверяет, находится ли дрон в режиме `Offboard` (`self.offboardMode == True`). Если да, он формирует сообщение `OffboardControlMode` и отправляет его. + - Затем вычисляется и отправляется сообщение `TrajectorySetpoint`, которое содержит информацию о требуемой скорости дрона и угловом повороте (yaw). + +3. **Метод `offboard_velocity_callback`**: + - Этот метод вызывается каждый раз, когда в топик `/offboard_velocity_cmd` поступает новое сообщение `Twist`. + - Метод преобразует поступающие команды `Twist` в систему координат FLU (Forward-Left-Up) и сохраняет их в переменных `self.velocity` и `self.yaw`. + - Эти значения затем используются в `cmdloop_callback` для управления движением дрона. + +4. **Метод `vehicle_status_callback`**: + - Этот метод вызывается при получении сообщений из топика `/fmu/out/vehicle_status`. + - Он обновляет информацию о текущем состоянии дрона, включая навигационный статус (`nav_state`), состояние армирования (`arm_state`), наличие аварийной ситуации (`failsafe`) и результат проверки перед полетом (`flightCheck`). + - Эти значения влияют на переходы между состояниями в методе `arm_timer_callback`. + +5. **Метод `attitude_callback`**: + - Этот метод вызывается при получении сообщений из топика `/fmu/out/vehicle_attitude`. + - Он вычисляет текущий угол рыскания (yaw) дрона на основе полученного сообщения и сохраняет его в переменной `self.trueYaw`. + - Значение `trueYaw` используется в `cmdloop_callback` для правильного преобразования команд скорости в мировую систему координат. + +6. **Метод `publish_vehicle_command`**: + - Этот метод вызывается для отправки команд в топик `/fmu/in/vehicle_command`. Команды могут включать армирование/разармирование дрона, команды взлета и перехода в режим `Offboard`. + - Метод формирует и публикует сообщение `VehicleCommand`, которое содержит необходимые параметры для выполнения команды. + +7. **Методы состояний (`state_init`, `state_arming`, `state_takeoff`, `state_loiter`, `state_offboard`)**: + - Эти методы реализуют конкретные действия, выполняемые в каждом состоянии FSM: + - `state_init`: Инициализация состояния, сброс счетчика. + - `state_arming`: Отправка команды на армирование дрона. + - `state_takeoff`: Отправка команды на взлет на заданную высоту. + - `state_loiter`: Ожидание завершения взлета и перехода в режим зависания (LOITER). + - `state_offboard`: Отправка команды на переход в режим `Offboard` и включение этого режима. + +### Ход выполнения в общем + +1. **Инициализация и запуск узла:** + - Запускается ROS 2 узел, создаются подписки, издатели и таймеры. + +2. **Обработка сообщений:** + - По мере поступления сообщений (статус дрона, команды с клавиатуры, данные о положении) вызываются соответствующие callback-функции, которые обновляют состояние контроллера и отправляют команды дрону. + +3. **Цикл состояния дрона:** + - В цикле состояния (`arm_timer_callback`) происходит проверка текущего состояния дрона и, при необходимости, переходы в следующие состояния (армирование, взлет, зависание, управление). + +4. **Отправка команд управления:** + - В цикле `cmdloop_callback` проверяется, находится ли дрон в режиме `Offboard`, и если да, отправляются команды на движение на основе входных данных. \ No newline at end of file diff --git a/launch/aerial_drone_base.py b/launch/aerial_drone_base.py index 94a1714..0683646 100644 --- a/launch/aerial_drone_base.py +++ b/launch/aerial_drone_base.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +# !/usr/bin/env python3 import os from launch import LaunchDescription @@ -10,7 +10,7 @@ def generate_launch_description(): # Название пакета - pkg_name = 'delta_robot_ros2' + pkg_name = 'aerial_drone_base' pkg_share = get_package_share_directory(pkg_name) @@ -30,39 +30,40 @@ def generate_launch_description(): ) visualizer_node = Node( - package='delta_robot_ros2', - namespace='delta_robot_ros2', - executable='visualizer', + package='aerial_drone_base', + namespace='aerial_drone_base', + executable='visualizer.py', name='visualizer' ) processes_node = Node( - package='delta_robot_ros2', - namespace='delta_robot_ros2', - executable='processes', + package=pkg_name, + namespace='aerial_drone_base', + executable='processes.py', name='processes', - prefix='tmux new-session -d -s mysession "bash -c \'', # Создание новой сессии tmux и выполнение команды + output='screen', ) control_node = Node( - package='delta_robot_ros2', - namespace='delta_robot_ros2', - executable='control', + package='aerial_drone_base', + namespace='aerial_drone_base', + executable='control.py', name='control', - prefix='tmux new-window -t mysession:1 "bash -c \'', # Создание нового окна tmux в существующей сессии + prefix="gnome-terminal --", ) velocity_node = Node( - package='delta_robot_ros2', - namespace='delta_robot_ros2', - executable='velocity_control', - name='velocity' + package='aerial_drone_base', + namespace='aerial_drone_base', + executable='velocity_control.py', + name='velocity', + prefix="gnome-terminal --", ) # Список нод nodes = [ - rviz_node, - visualizer_node, + # rviz_node, + # visualizer_node, processes_node, control_node, velocity_node, diff --git a/scripts/control.py b/scripts/control.py index 8ea700a..a6ff98d 100644 --- a/scripts/control.py +++ b/scripts/control.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 + import sys import geometry_msgs.msg # Импортируем сообщения для задания геометрических данных (скорости) @@ -16,19 +17,29 @@ # Сообщение, которое выводится при запуске программы msg = """ -This node takes keypresses from the keyboard and publishes them +This node takes key presses from the keyboard and publishes them as Twist messages. -Using the arrow keys and WASD you have Mode 2 RC controls. +Using the arrow keys and WASD, you can control the drone. + +Controls: W: Up S: Down A: Yaw Left D: Yaw Right -Up Arrow: Pitch Forward -Down Arrow: Pitch Backward +Up Arrow: Forward +Down Arrow: Backward Left Arrow: Roll Left Right Arrow: Roll Right -Press SPACE to arm/disarm the drone +Space: Arm/Disarm the drone + +Speed adjustments: +Q: Increase linear and angular speed +Z: Decrease linear and angular speed +E: Increase angular speed +C: Decrease angular speed + +Press CTRL+C to exit """ # Словарь для сопоставления клавиш с командами движения diff --git a/scripts/processes.py b/scripts/processes.py index 9cce305..d5ee5bd 100644 --- a/scripts/processes.py +++ b/scripts/processes.py @@ -1,35 +1,95 @@ #!/usr/bin/env python3 -# Импортируем модули subprocess и time import subprocess # Модуль для выполнения системных команд и запуска процессов import time # Модуль для работы с временем (задержки) +import rclpy +from rclpy.node import Node -# Список команд для выполнения -commands = [ - # Запуск Micro XRCE-DDS Agent - "MicroXRCEAgent udp4 -p 8888", - - # Запуск симуляции PX4 SITL - "cd /workspace/src/PX4-Autopilot && make px4_sitl gz_x500", - - # Запуск QGroundControl (закомментировано) - # "cd ~/QGroundControl && ./QGroundControl.AppImage" -] - -# Инициализируем tmux сессию -subprocess.run(["tmux", "new-session", "-d", "-s", "mysession"]) - -# Проход по каждому элементу в списке команд -for i, command in enumerate(commands): - if i == 0: - # Первая команда запускается в первом окне сессии tmux - subprocess.run(["tmux", "send-keys", "-t", "mysession:0", command + "; exec bash", "C-m"]) - else: - # Последующие команды запускаются в новых окнах tmux - subprocess.run(["tmux", "new-window", "-t", f"mysession:{i}", "-n", f"window{i}", "bash", "-c", command + "; exec bash"]) +class ProcessesNode(Node): + """ + Класс ProcessesNode представляет собой узел ROS 2, предназначенный для управления запуском и завершением + нескольких процессов, связанных с управлением беспилотным летательным аппаратом (БПЛА). + + Основные функции класса: + + 1. Инициализация узла ROS 2 и запуск процессов через `gnome-terminal`. + 2. Остановка всех предыдущих процессов PX4 и Gazebo. + 3. Создание новых вкладок `gnome-terminal` для параллельного выполнения команд. + 4. Управление процессами, включая завершение процессов по окончании работы узла. + 5. Создание таймера для периодического выполнения задач (если необходимо). + """ - # Задержка перед запуском следующей команды (1 секунда) - time.sleep(1) + def __init__(self): + super().__init__('processes_node') + self.get_logger().info('Processes node has been started') + + # Список команд для выполнения + self.commands = [ + # Запуск Micro XRCE-DDS Agent + "MicroXRCEAgent udp4 -p 8888", + + # Запуск симуляции PX4 SITL + "cd /workspace/src/PX4-Autopilot && make px4_sitl gz_x500", + + # Запуск QGroundControl (закомментировано) + # "cd ~/QGroundControl && ./QGroundControl.AppImage" + ] + + # Остановка предыдущих процессов перед запуском новых + self.clean_previous_sessions() + + # Инициализация gnome-terminal и запуск процессов + self.start_gnome_terminal_sessions() + + # Создаем таймер для периодической проверки + self.timer = self.create_timer(1.0, self.timer_callback) + + def clean_previous_sessions(self): + """Завершаем все процессы PX4 и Gazebo, которые могут быть запущены.""" + self.get_logger().info("Cleaning up any previous PX4 and Gazebo processes...") + + # Завершаем процессы PX4 + subprocess.run(["pkill", "-f", "px4"]) + + # Завершаем процессы Gazebo + subprocess.run(["pkill", "-f", "gz"]) + subprocess.run(["pkill", "-f", "gzserver"]) + subprocess.run(["pkill", "-f", "gzclient"]) + time.sleep(1) # Подождем, чтобы процессы завершились + + def start_gnome_terminal_sessions(self): + """Запускаем процессы в новых вкладках gnome-terminal.""" + + for i, command in enumerate(self.commands): + self.get_logger().info(f"Starting gnome-terminal tab and running command: {command}") + subprocess.run([ + "gnome-terminal", "--tab", "--", "bash", "-c", command + "; exec bash" + ]) + time.sleep(1) # Задержка перед запуском следующей команды + + def timer_callback(self): + """Эта функция будет вызвана периодически по таймеру, если нужно выполнять какие-то действия.""" + pass + + def shutdown_gnome_terminal_sessions(self): + """Завершаем процессы gnome-terminal.""" + self.get_logger().info('Attempting to stop gnome-terminal processes') + # Если необходимо, вы можете реализовать логику для завершения запущенных процессов + +def main(args=None): + rclpy.init(args=args) + node = ProcessesNode() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info('Processes node interrupted') + finally: + # Обязательно вызываем завершение процессов + node.shutdown_gnome_terminal_sessions() + # Завершаем работу ROS контекста только после всех других действий + if rclpy.ok(): + rclpy.shutdown() -# Команда для подключения к сессии tmux (по желанию) -# subprocess.run(["tmux", "attach-session", "-t", "mysession"]) +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/scripts/velocity_control.py b/scripts/velocity_control.py index 705242a..e10b30c 100644 --- a/scripts/velocity_control.py +++ b/scripts/velocity_control.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 ############################################################################ # @@ -34,19 +34,20 @@ ############################################################################ # Импортируем необходимые библиотеки +import time # Импортируем модуль для добавления пауз + +import numpy as np # Библиотека для работы с массивами и математическими функциями + import rclpy # Основная библиотека для работы с ROS 2 from rclpy.node import Node # Класс для создания узла ROS 2 -import numpy as np # Библиотека для работы с массивами и математическими функциями from rclpy.clock import Clock # Класс для работы с временем в ROS 2 from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy # Настройки QoS # Импортируем необходимые сообщения из пакетов PX4 и ROS from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleStatus, VehicleAttitude, VehicleCommand from geometry_msgs.msg import Twist, Vector3 -from math import pi # Импортируем pi для математических расчетов from std_msgs.msg import Bool # Импортируем Bool для обработки сообщений булевого типа -# Класс для управления дроном в режиме Offboard class OffboardControl(Node): def __init__(self): @@ -54,9 +55,9 @@ def __init__(self): # Настройка QoS для подписок и публикаций qos_profile = QoSProfile( - reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + history=QoSHistoryPolicy.KEEP_LAST, depth=1 ) @@ -92,11 +93,11 @@ def __init__(self): self.vehicle_command_publisher_ = self.create_publisher(VehicleCommand, "/fmu/in/vehicle_command", 10) # Создаем таймер для периодической отправки команд на армирование - arm_timer_period = .1 # seconds + arm_timer_period = 2 # seconds self.arm_timer_ = self.create_timer(arm_timer_period, self.arm_timer_callback) # Создаем таймер для цикла управления дроном - timer_period = 0.02 # seconds + timer_period = 2 # seconds self.timer = self.create_timer(timer_period, self.cmdloop_callback) # Инициализируем переменные состояния дрона и управления @@ -122,101 +123,102 @@ def __init__(self): self.current_state = "IDLE" # Начальное состояние self.last_state = self.current_state # Предыдущее состояние - # Callback-функция для обработки сообщений об арминге дрона def arm_message_callback(self, msg): self.arm_message = msg.data - self.get_logger().info(f"Arm Message: {self.arm_message}") + self.get_logger().info(f"Received Arm Message: {self.arm_message}") # Callback-функция для управления состояниями дрона (автомат) def arm_timer_callback(self): + self.get_logger().info(f"Current State: {self.current_state}, Arm State: {self.arm_state}, Nav State: {self.nav_state}") match self.current_state: case "IDLE": - if(self.flightCheck and self.arm_message == True): + if self.flightCheck and self.arm_message: self.current_state = "ARMING" - self.get_logger().info(f"Arming") + self.get_logger().info("Transitioning to ARMING state") case "ARMING": - if(not(self.flightCheck)): + if not self.flightCheck: self.current_state = "IDLE" - self.get_logger().info(f"Arming, Flight Check Failed") - elif(self.arm_state == VehicleStatus.ARMING_STATE_ARMED and self.myCnt > 10): + self.get_logger().info("Flight check failed, transitioning to IDLE") + elif self.arm_state == VehicleStatus.ARMING_STATE_ARMED and self.myCnt > 10: self.current_state = "TAKEOFF" - self.get_logger().info(f"Arming, Takeoff") + self.get_logger().info("Armed successfully, transitioning to TAKEOFF") self.arm() # Отправляем команду на армирование case "TAKEOFF": - if(not(self.flightCheck)): + if not self.flightCheck: self.current_state = "IDLE" - self.get_logger().info(f"Takeoff, Flight Check Failed") - elif(self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_TAKEOFF): + self.get_logger().info("Flight check failed, transitioning to IDLE") + elif self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_TAKEOFF: self.current_state = "LOITER" - self.get_logger().info(f"Takeoff, Loiter") + self.get_logger().info("Takeoff successful, transitioning to LOITER") self.arm() # Отправляем команду на армирование self.take_off() # Отправляем команду на взлет - # Ожидание в состоянии LOITER, пока не будет достигнуто состояние LOITER case "LOITER": - if(not(self.flightCheck)): + if not self.flightCheck: self.current_state = "IDLE" - self.get_logger().info(f"Loiter, Flight Check Failed") - elif(self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_LOITER): + self.get_logger().info("Flight check failed, transitioning to IDLE") + elif self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_LOITER: self.current_state = "OFFBOARD" - self.get_logger().info(f"Loiter, Offboard") + self.get_logger().info("Loiter successful, transitioning to OFFBOARD") self.arm() case "OFFBOARD": - if(not(self.flightCheck) or self.arm_state != VehicleStatus.ARMING_STATE_ARMED or self.failsafe == True): + if not self.flightCheck or self.arm_state != VehicleStatus.ARMING_STATE_ARMED or self.failsafe: self.current_state = "IDLE" - self.get_logger().info(f"Offboard, Flight Check Failed") + self.get_logger().info("Offboard failed, transitioning to IDLE") self.state_offboard() - if(self.arm_state != VehicleStatus.ARMING_STATE_ARMED): + if self.arm_state != VehicleStatus.ARMING_STATE_ARMED: self.arm_message = False - if (self.last_state != self.current_state): + if self.last_state != self.current_state: self.last_state = self.current_state - self.get_logger().info(self.current_state) + self.get_logger().info(f"State changed to: {self.current_state}") self.myCnt += 1 # Функции для инициализации состояния def state_init(self): self.myCnt = 0 + self.get_logger().info("Initializing state") # Функция для состояния ARMED def state_arming(self): self.myCnt = 0 self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0) - self.get_logger().info("Arm command send") + self.get_logger().info("Sending arm command") # Функция для состояния TAKEOFF def state_takeoff(self): self.myCnt = 0 - self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param1 = 1.0, param7=5.0) # param7 - высота в метрах - self.get_logger().info("Takeoff command send") + self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param1=1.0, param7=5.0) # param7 - высота в метрах + self.get_logger().info("Sending takeoff command") # Функция для состояния LOITER def state_loiter(self): self.myCnt = 0 - self.get_logger().info("Loiter Status") + self.get_logger().info("Loiter Status: Waiting for transition") # Функция для состояния OFFBOARD def state_offboard(self): self.myCnt = 0 self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1., 6.) self.offboardMode = True + self.get_logger().info("Offboard mode engaged") # Функция для отправки команды на армирование def arm(self): self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0) - self.get_logger().info("Arm command send") + self.get_logger().info("Arm command sent") # Функция для отправки команды на взлет на заданную высоту def take_off(self): - self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param1 = 1.0, param7=5.0) # param7 - высота в метрах - self.get_logger().info("Takeoff command send") + self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param1=1.0, param7=5.0) # param7 - высота в метрах + self.get_logger().info("Takeoff command sent") # Публикует команду на топик /fmu/in/vehicle_command def publish_vehicle_command(self, command, param1=0.0, param2=0.0, param7=0.0): @@ -232,21 +234,21 @@ def publish_vehicle_command(self, command, param1=0.0, param2=0.0, param7=0.0): msg.from_external = True msg.timestamp = int(Clock().now().nanoseconds / 1000) # Время в микросекундах self.vehicle_command_publisher_.publish(msg) + self.get_logger().info(f"Published VehicleCommand: command={command}, param1={param1}, param2={param2}, param7={param7}") # Callback-функция для получения и установки значений статуса дрона def vehicle_status_callback(self, msg): - - if (msg.nav_state != self.nav_state): - self.get_logger().info(f"NAV_STATUS: {msg.nav_state}") + if msg.nav_state != self.nav_state: + self.get_logger().info(f"NAV_STATUS changed: {msg.nav_state}") - if (msg.arming_state != self.arm_state): - self.get_logger().info(f"ARM STATUS: {msg.arming_state}") + if msg.arming_state != self.arm_state: + self.get_logger().info(f"ARM_STATUS changed: {msg.arming_state}") - if (msg.failsafe != self.failsafe): - self.get_logger().info(f"FAILSAFE: {msg.failsafe}") + if msg.failsafe != self.failsafe: + self.get_logger().info(f"FAILSAFE status changed: {msg.failsafe}") - if (msg.pre_flight_checks_pass != self.flightCheck): - self.get_logger().info(f"FlightCheck: {msg.pre_flight_checks_pass}") + if msg.pre_flight_checks_pass != self.flightCheck: + self.get_logger().info(f"FlightCheck status changed: {msg.pre_flight_checks_pass}") self.nav_state = msg.nav_state self.arm_state = msg.arming_state @@ -255,30 +257,38 @@ def vehicle_status_callback(self, msg): # Callback-функция для обработки команд Twist из Teleop и преобразования их в систему координат FLU def offboard_velocity_callback(self, msg): + # Логирование полученного сообщения Twist + # self.get_logger().info(f"Received Twist message: linear=({msg.linear.x}, {msg.linear.y}, {msg.linear.z}), angular=({msg.angular.x}, {msg.angular.y}, {msg.angular.z})") + # Преобразование NED -> FLU self.velocity.x = -msg.linear.y self.velocity.y = msg.linear.x self.velocity.z = -msg.linear.z self.yaw = msg.angular.z + # self.get_logger().info(f"Converted velocity: x={self.velocity.x}, y={self.velocity.y}, z={self.velocity.z}, yaw={self.yaw}") + # Callback-функция для получения текущих значений траектории и извлечения угла рыскания def attitude_callback(self, msg): orientation_q = msg.q # trueYaw - текущее значение угла рыскания дрона - self.trueYaw = -(np.arctan2(2.0*(orientation_q[3]*orientation_q[0] + orientation_q[1]*orientation_q[2]), - 1.0 - 2.0*(orientation_q[0]*orientation_q[0] + orientation_q[1]*orientation_q[1]))) - + self.trueYaw = -(np.arctan2(2.0 * (orientation_q[3] * orientation_q[0] + orientation_q[1] * orientation_q[2]), + 1.0 - 2.0 * (orientation_q[0] * orientation_q[0] + orientation_q[1] * orientation_q[1]))) + + # self.get_logger().info(f"Received attitude: trueYaw={self.trueYaw}") + # Callback-функция для публикации режимов управления Offboard и скорости в качестве точек траектории def cmdloop_callback(self): - if(self.offboardMode == True): + if self.offboardMode: # Публикуем режимы управления Offboard offboard_msg = OffboardControlMode() offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000) offboard_msg.position = False offboard_msg.velocity = True offboard_msg.acceleration = False - self.publisher_offboard_mode.publish(offboard_msg) + self.publisher_offboard_mode.publish(offboard_msg) + self.get_logger().info("Published OffboardControlMode") # Вычисляем скорость в системе координат мира cos_yaw = np.cos(self.trueYaw) @@ -302,6 +312,7 @@ def cmdloop_callback(self): trajectory_msg.yawspeed = self.yaw self.publisher_trajectory.publish(trajectory_msg) + self.get_logger().info(f"Published TrajectorySetpoint: velocity=({velocity_world_x}, {velocity_world_y}, {self.velocity.z}), yaw={self.yaw}") # Главная функция для запуска узла def main(args=None): @@ -314,6 +325,5 @@ def main(args=None): offboard_control.destroy_node() # Завершение работы узла rclpy.shutdown() # Остановка ROS 2 - if __name__ == '__main__': main() # Запуск главной функции diff --git a/scripts/visualizer.py b/scripts/visualizer.py index 3f5b868..12a0038 100644 --- a/scripts/visualizer.py +++ b/scripts/visualizer.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 ############################################################################ # @@ -33,7 +33,6 @@ # ############################################################################ -from re import M import numpy as np # Импортируем numpy для работы с массивами и векторами import rclpy # Импортируем библиотеку для работы с ROS 2