Run drone with control

.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",
-	],
-}
+    // Рабочая директория внутри контейнера
+    "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
+}

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)

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 и устанавливаем его

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

docs/commands.md

@@ -23,12 +23,15 @@ MicroXRCEAgent udp4 -p 8888
 reboot now
 
 ros2 topic echo /fmu/out/vehicle_status
-```
+
 
 ENV PX4_HOME_LAT=50
 ENV PX4_HOME_LON=23
 ENV PX4_HOME_ALT=100
 
 docker inspect -f ros_2_humle-ros-humble-master-1
 
-export PX4_SIM_HOSTNAME=172.26.176.1
+export PX4_SIM_HOSTNAME=172.26.176.1
+
+
+```

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`, и если да, отправляются команды на движение на основе входных данных.

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,

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
 """
 
 # Словарь для сопоставления клавиш с командами движения