diff --git a/builder/assets/examples/navigate_wait.py b/builder/assets/examples/navigate_wait.py index 7ead3be8..baef761b 100644 --- a/builder/assets/examples/navigate_wait.py +++ b/builder/assets/examples/navigate_wait.py @@ -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 diff --git a/docs/en/frames.md b/docs/en/frames.md index c3840d96..7eab6f12 100644 --- a/docs/en/frames.md +++ b/docs/en/frames.md @@ -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); +* `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: diff --git a/docs/en/programming.md b/docs/en/programming.md index 5967189a..cbd04bc3 100644 --- a/docs/en/programming.md +++ b/docs/en/programming.md @@ -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. diff --git a/docs/en/snippets.md b/docs/en/snippets.md index 90939963..7ca5ee7c 100644 --- a/docs/en/snippets.md +++ b/docs/en/snippets.md @@ -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)): + -```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} + 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} + + 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} diff --git a/docs/ru/frames.md b/docs/ru/frames.md index cdf593ca..6bc760fc 100644 --- a/docs/ru/frames.md +++ b/docs/ru/frames.md @@ -10,7 +10,7 @@ * `map` — координаты относительно точки инициализации полетного контроллера: белая сетка на иллюстрации; * `base_link` — координаты относительно квадрокоптера: схематичное изображение квадрокоптера на иллюстрации; * `body` — координаты относительно квадрокоптера без учета наклонов по тангажу и крену: красная, синяя и зеленая линии на иллюстрации; -* `navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate)); +* `navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate)); * `setpoint` – текущий setpoint по позиции. При использовании [системы позиционирования по ArUco-маркерам](aruco.md) появляются дополнительные фреймы: diff --git a/docs/ru/programming.md b/docs/ru/programming.md index a0bd0ab4..995f133e 100644 --- a/docs/ru/programming.md +++ b/docs/ru/programming.md @@ -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). diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index eb328ac5..0dcb5ed8 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -13,7 +13,59 @@ Python -### # {#distance} +### # {#navigate_wait} + + + + + +Полет в точку и ожидание окончания полета: + +```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} + + + +Посадка и ожидание окончания посадки: + +```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} Дизарм коптера (выключение винтов, **коптер упадет**):