docs: simplify and fix some snippets

This commit is contained in:
Oleg Kalachev
2022-01-28 06:20:57 +03:00
parent e7eae1c02d
commit 1aec5063d6
2 changed files with 12 additions and 71 deletions

View File

@@ -17,13 +17,11 @@
<a name="block-takeoff"></a><!-- old name of anchor -->
Полет в точку и ожидание окончания полета:
Функция для полета в точку и ожидание окончания полета:
```python
import math
# ...
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)
@@ -74,8 +72,6 @@ land_wait()
```python
import math
# ...
def wait_arrival(tolerance=0.2):
while not rospy.is_shutdown():
telem = get_telemetry(frame_id='navigate_target')
@@ -91,8 +87,6 @@ def wait_arrival(tolerance=0.2):
```python
import math
# ...
def get_distance(x1, y1, z1, x2, y2, z2):
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
```
@@ -104,8 +98,6 @@ def get_distance(x1, y1, z1, x2, y2, z2):
```python
import math
# ...
def get_distance_global(lat1, lon1, lat2, lon2):
return math.hypot(lat1 - lat2, lon1 - lon2) * 1.113195e5
```
@@ -221,19 +213,16 @@ from geometry_msgs.msg import PoseStamped, TwistStamped
from sensor_msgs.msg import BatteryState
from mavros_msgs.msg import RCIn
# ...
def pose_update(pose):
# Обработка новых данных о позиции коптера
pass
# Остальные функции-обработчики
# ...
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
rospy.Subscriber('mavros/rc/in', RCIn, rc_callback)
rospy.spin()
```
Информацию по топикам MAVROS см. по [ссылке](mavros.md).
@@ -247,14 +236,10 @@ rospy.Subscriber('mavros/rc/in', RCIn, rc_callback)
Пример отправки произвольного [MAVLink-сообщения](mavlink.md) коптеру:
```python
# ...
from mavros_msgs.msg import Mavlink
from mavros import mavlink
from pymavlink import mavutil
# ...
mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
# Отправка сообщения HEARTBEAT:
@@ -299,8 +284,6 @@ rospy.spin()
```python
from mavros_msgs.srv import SetMode
# ...
set_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
# ...
@@ -315,8 +298,6 @@ set_mode(custom_mode='STABILIZED')
```python
import math
# ...
PI_2 = math.pi / 2
def flip():
@@ -355,8 +336,6 @@ from pymavlink import mavutil
from mavros_msgs.srv import CommandLong
from mavros_msgs.msg import State
# ...
send_command = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
def calibrate_gyro():
@@ -390,16 +369,14 @@ calibrate_gyro()
import rospy
import dynamic_reconfigure.client
# ...
client = dynamic_reconfigure.client.Client('aruco_detect')
# Turn markers recognition off
# Включить распознавание маркеров
client.update_configuration({'enabled': False})
rospy.sleep(5)
# Turn markers recognition on
# Выключить распознавание маркеров
client.update_configuration({'enabled': True})
```
@@ -410,8 +387,6 @@ client.update_configuration({'enabled': True})
```python
import math
# ...
while not rospy.is_shutdown():
if math.isfinite(get_telemetry().lat):
break
@@ -426,12 +401,8 @@ while not rospy.is_shutdown():
from mavros_msgs.srv import ParamGet
from mavros_msgs.msg import ParamValue
# ...
param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
# ...
# Считать параметр типа INT
value = param_get(param_id='COM_FLTMODE1').value.integer
@@ -447,12 +418,8 @@ value = param_get(param_id='MPC_Z_P').value.float
from mavros_msgs.srv import ParamSet
from mavros_msgs.msg import ParamValue
# ...
param_set = rospy.ServiceProxy('mavros/param/set', ParamSet)
# ...
# Изменить параметр типа INT:
param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8))