CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

docs: rework and simplify navigate_wait snippet, move it on top

Browse Source
This commit is contained in:
Oleg Kalachev
2020-08-21 18:16:25 +03:00
parent 106209d79b
commit d4d25c61a2
7 changed files with 99 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
builder/assets/examples/navigate_wait.py
View File

@@ -1,4 +1,4 @@
# Information: https://clover.coex.tech/en/snippets.html#block-nav
# Information: https://clover.coex.tech/en/snippets.html#navigate_wait
import math
import rospy

2
docs/en/frames.md
View File

@@ -8,7 +8,7 @@ Main frames in the `clover` package:
* `map` has its origin at the flight controller initialization point and may be considered stationary. It is shown as a white grid on the image above;
* `base_link` is rigidly bound to the drone. It is shown by the simplified drone model on the image above;
* `body` is bound to the drone, but its Z axis points up regardless of the drone's pitch and roll. It is shown by the red, blue and green lines in the illustration;
* `navigate_target` is bound to the current navigation target (as set by the [navigate](simple_offboard.md#navigate) service);
* <a name="navigate_target"></a>`navigate_target` is bound to the current navigation target (as set by the [navigate](simple_offboard.md#navigate) service);
* `setpoint` is current position setpoint.
Additional frames become available when [ArUco positioning system](aruco.md) is active:

2
docs/en/programming.md
View File

@@ -76,7 +76,7 @@ rospy.sleep(3)
land()
```
> **Note** The `navigate` function call is not blocking; that is, the program will continue executing the next commands before the drone arrives at the set point. Look at the [`navigate_wait`](snippets.md#block-nav) snippet for a blocking function.
> **Note** The `navigate` function call is not blocking; that is, the program will continue executing the next commands before the drone arrives at the set point. Look at the [`navigate_wait`](snippets.md#navigate_wait) snippet for a blocking function.
Note that only the first `navigate` call has its `auto_arm` parameter set to `True`. This parameter arms the drone and transitions it to the OFFBOARD flight mode.

134
docs/en/snippets.md
View File

@@ -4,100 +4,17 @@ Code examples
Python
---
### # {#distance}
### # {#navigate_wait}
Calculating the distance between two points (**important**: the points are to be in the same [system of coordinates](frames.md)):
<a name="block-nav"></a><!-- old name of anchor -->
```python
def get_distance(x1, y1, z1, x2, y2, z2):
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
```
### # {#distance-global}
Approximation of distance (in meters) between two global coordinates (latitude/longitude):
```python
def get_distance_global(lat1, lon1, lat2, lon2):
return math.hypot(lat1 - lat2, lon1 - lon2) * 1.113195e5
```
### # {#block-takeoff}
Takeoff and waiting for it to finish:
```python
z = 2 # altitude
tolerance = 0.2 # precision of altitude check (m)
# Saving the initial point
start = get_telemetry()
# Take off and leveling at 2 m above the ground
print navigate(z=z, speed=0.5, frame_id='body', auto_arm=True)
# Waiting for takeoff
while not rospy.is_shutdown():
# Checking current altitude
if start.z + z - get_telemetry().z < tolerance:
# Takeoff complete
break
rospy.sleep(0.2)
```
This code can be wrapped in a function:
```python
def takeoff_wait(alt, speed=0.5, tolerance=0.2):
start = get_telemetry()
print navigate(z=alt, speed=speed, frame_id='body', auto_arm=True)
while not rospy.is_shutdown():
if start.z + alt - get_telemetry().z < tolerance:
break
rospy.sleep(0.2)
```
### # {#block-nav}
<a name="block-takeoff"></a><!-- old name of anchor -->
Flying towards a point and waiting for copter's arrival:
```python
tolerance = 0.2 # precision of arrival check (m)
frame_id='aruco_map'
# Flying to point 1:2:3 in the field of ArUco markers
print navigate(frame_id=frame_id, x=1, y=2, z=3, speed=0.5)
# Wait for the copter to arrive at the requested point
while not rospy.is_shutdown():
telem = get_telemetry(frame_id=frame_id)
# Calculating the distance to the requested point
if get_distance(1, 2, 3, telem.x, telem.y, telem.z) < tolerance:
# Arrived at the requested point
break
rospy.sleep(0.2)
```
This code can be wrapped into a function:
```python
def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id)
while not rospy.is_shutdown():
telem = get_telemetry(frame_id=frame_id)
if get_distance(x, y, z, telem.x, telem.y, telem.z) < tolerance:
break
rospy.sleep(0.2)
```
A more universal solution, utilizing the `navigate_target` frame, which corresponds to the navigating target point of the drone:
```python
def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id)
def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), speed=0.5, frame_id='', auto_arm=False, tolerance=0.2):
navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
while not rospy.is_shutdown():
telem = get_telemetry(frame_id='navigate_target')
@@ -106,10 +23,24 @@ def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
rospy.sleep(0.2)
```
This code also can be used for navigating using `body` frame.
This function utilizes [`navigate_target`](frames.md#navigate_target) frame for computing the distance to the target.
Using the function for flying to the point x=3, y=2, z=1 in [marker's map](aruco_map.md):
```python
navigate_wait(x=3, y=2, z=1, frame_id='aruco_map')
```
This function can be used for taking off as well:
```python
navigate_wait(z=1, frame_id='body', auto_arm=True)
```
### # {#block-land}
<a name="block-land"></a><!-- old name of anchor -->
Landing and waiting until the copter lands:
```python
@@ -118,13 +49,28 @@ while get_telemetry().armed:
rospy.sleep(0.2)
```
This code can be wrapped in a function:
Usage:
```python
def land_wait():
land()
while get_telemetry().armed:
rospy.sleep(0.2)
land_wait()
```
### # {#get_distance}
Calculating the distance between two points (**important**: the points are to be in the same [system of coordinates](frames.md)):
```python
def get_distance(x1, y1, z1, x2, y2, z2):
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
```
### # {#get_distance_global}
Approximation of distance (in meters) between two global coordinates (latitude/longitude):
```python
def get_distance_global(lat1, lon1, lat2, lon2):
return math.hypot(lat1 - lat2, lon1 - lon2) * 1.113195e5
```
### # {#disarm}

2
docs/ru/frames.md
View File

@@ -10,7 +10,7 @@
* `map` — координаты относительно точки инициализации полетного контроллера: белая сетка на иллюстрации;
* `base_link` — координаты относительно квадрокоптера: схематичное изображение квадрокоптера на иллюстрации;
* `body` — координаты относительно квадрокоптера без учета наклонов по тангажу и крену: красная, синяя и зеленая линии на иллюстрации;
* `navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate));
* <a name="navigate_target"></a>`navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate));
* `setpoint` – текущий setpoint по позиции.
При использовании [системы позиционирования по ArUco-маркерам](aruco.md) появляются дополнительные фреймы:

2
docs/ru/programming.md
View File

@@ -76,7 +76,7 @@ rospy.sleep(3)
land()
```
> **note** Функция navigate не ожидает, пока дрон долетит до целевой точки; скрипт продолжит выполнение сразу. Для блокирующей версии смотрите пример функции [`navigate_wait`](snippets.md#block-nav).
> **note** Функция navigate не ожидает, пока дрон долетит до целевой точки; скрипт продолжит выполнение сразу. Для блокирующей версии смотрите пример функции [`navigate_wait`](snippets.md#navigate_wait).
Обратите внимание, что параметр `auto_arm` установлен на `True` только у первого вызова функции `navigate`. Этот параметр армит дрон и переводит его в режим автономного полета (OFFBOARD).

161
docs/ru/snippets.md
View File

@@ -13,7 +13,59 @@ Python
<!-- markdownlint-enable MD031 -->
### # {#distance}
### # {#navigate_wait}
<a name="block-nav"></a><!-- old name of anchor -->
<a name="block-takeoff"></a><!-- old name of anchor -->
Полет в точку и ожидание окончания полета:
```python
def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), speed=0.5, frame_id='', auto_arm=False, tolerance=0.2):
navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
while not rospy.is_shutdown():
telem = get_telemetry(frame_id='navigate_target')
if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
break
rospy.sleep(0.2)
```
Для того, чтобы определить расстояние до целевой точки, функция использует фрейм [`navigate_target`](frames.md#navigate_target).
Использование функции для полета в точку x=3, y=2, z=1 [относительно карты маркеров](aruco_map.md):
```python
navigate_wait(x=3, y=2, z=1, frame_id='aruco_map')
```
Эту функцию можно использовать и для взлета:
```python
navigate_wait(z=1, frame_id='body', auto_arm=True)
```
### # {#land_wait}
<a name="block-land"></a><!-- old name of anchor -->
Посадка и ожидание окончания посадки:
```python
def land_wait():
land()
while get_telemetry().armed:
rospy.sleep(0.2)
```
Использование:
```python
land_wait()
```
### # {#get_distance}
Функция определения расстояния между двумя точками (**важно**: точки должны быть в одной [системе координат](frames.md)):
@@ -26,7 +78,7 @@ def get_distance(x1, y1, z1, x2, y2, z2):
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
```
### # {#distance-global}
### # {#get_distance_global}
Функция для приблизительного определения расстояния (в метрах) между двумя глобальными координатами (широта/долгота):
@@ -39,111 +91,6 @@ def get_distance_global(lat1, lon1, lat2, lon2):
return math.hypot(lat1 - lat2, lon1 - lon2) * 1.113195e5
```
### # {#block-takeoff}
Взлет и ожидание окончания взлета:
```python
z = 2 # высота
tolerance = 0.2 # точность проверки высоты (м)
# Запоминаем изначальную точку
start = get_telemetry()
# Взлетаем на 2 м
print navigate(z=z, speed=0.5, frame_id='body', auto_arm=True)
# Ожидаем взлета
while not rospy.is_shutdown():
# Проверяем текущую высоту
if start.z + z - get_telemetry().z < tolerance:
# Взлет завершен
break
rospy.sleep(0.2)
```
Вышеприведенный код может быть обернут в функцию:
```python
def takeoff_wait(alt, speed=0.5, tolerance=0.2):
start = get_telemetry()
print navigate(z=alt, speed=speed, frame_id='body', auto_arm=True)
while not rospy.is_shutdown():
if start.z + alt - get_telemetry().z < tolerance:
break
rospy.sleep(0.2)
```
### # {#block-nav}
Лететь в точку и ждать пока коптер долетит в нее:
```python
tolerance = 0.2 # точность проверки прилета (м)
frame_id='aruco_map'
# Летим в точку 1:2:3 в поле ArUco-маркеров
print navigate(frame_id=frame_id, x=1, y=2, z=3, speed=0.5)
# Ждем, пока коптер долетит до запрошенной точки
while not rospy.is_shutdown():
telem = get_telemetry(frame_id=frame_id)
# Вычисляем расстояние до заданной точки
if get_distance(1, 2, 3, telem.x, telem.y, telem.z) < tolerance:
# Долетели до необходимой точки
break
rospy.sleep(0.2)
```
Вышеприведенный код может быть обернут в функцию:
```python
def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id)
while not rospy.is_shutdown():
telem = get_telemetry(frame_id=frame_id)
if get_distance(x, y, z, telem.x, telem.y, telem.z) < tolerance:
break
rospy.sleep(0.2)
```
Более универсальная функция с использованием фрейма `navigate_target`, который совпадает с целевой точкой навигации дрона:
```python
def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id)
while not rospy.is_shutdown():
telem = get_telemetry(frame_id='navigate_target')
if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
break
rospy.sleep(0.2)
```
Такой код может быть использован для полета в том числе с использованием фрейма `body`.
### # {#block-land}
Посадка и ожидание окончания посадки:
```python
land()
while get_telemetry().armed:
rospy.sleep(0.2)
```
Вышеприведенный код может быть обернут в функцию:
```python
def land_wait():
land()
while get_telemetry().armed:
rospy.sleep(0.2)
```
### # {#disarm}
Дизарм коптера (выключение винтов, **коптер упадет**):