mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-26 11:43:25 +00:00
docs: simplify and fix some snippets
This commit is contained in:
Oleg Kalachev
@@ -8,13 +8,11 @@
|
||||
|
||||
<a name="block-takeoff"></a><!-- old name of anchor -->
|
||||
|
||||
Fly towards a point and wait for copter's arrival:
|
||||
Function to fly to a point and wait for copter's arrival:
|
||||
|
||||
```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)
|
||||
|
||||
@@ -64,8 +62,6 @@ Wait for copter's arrival to the [navigate](simple_offboard.md#navigate) target:
|
||||
```python
|
||||
import math
|
||||
|
||||
# ...
|
||||
|
||||
def wait_arrival(tolerance=0.2):
|
||||
while not rospy.is_shutdown():
|
||||
telem = get_telemetry(frame_id='navigate_target')
|
||||
@@ -79,6 +75,8 @@ def wait_arrival(tolerance=0.2):
|
||||
Calculate the distance between two points (**important**: the points are to be in the same [coordinate system](frames.md)):
|
||||
|
||||
```python
|
||||
import math
|
||||
|
||||
def get_distance(x1, y1, z1, x2, y2, z2):
|
||||
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
|
||||
```
|
||||
@@ -88,6 +86,8 @@ def get_distance(x1, y1, z1, x2, y2, z2):
|
||||
Approximation of distance (in meters) between two global coordinates (latitude/longitude):
|
||||
|
||||
```python
|
||||
import math
|
||||
|
||||
def get_distance_global(lat1, lon1, lat2, lon2):
|
||||
return math.hypot(lat1 - lat2, lon1 - lon2) * 1.113195e5
|
||||
```
|
||||
@@ -203,19 +203,16 @@ from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from mavros_msgs.msg import RCIn
|
||||
|
||||
# ...
|
||||
|
||||
def pose_update(pose):
|
||||
# Processing new data of copter's position
|
||||
pass
|
||||
|
||||
# Other handler functions
|
||||
# ...
|
||||
|
||||
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()
|
||||
```
|
||||
|
||||
Information about MAVROS topics is available at [the link](mavros.md).
|
||||
@@ -229,18 +226,13 @@ Information about MAVROS topics is available at [the link](mavros.md).
|
||||
Send an arbitrary [MAVLink message](mavlink.md) to the copter:
|
||||
|
||||
```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)
|
||||
|
||||
# Sending a HEARTBEAT message:
|
||||
|
||||
msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0)
|
||||
msg.pack(mavutil.mavlink.MAVLink('', 2, 1))
|
||||
ros_msg = mavlink.convert_to_rosmsg(msg)
|
||||
@@ -281,8 +273,6 @@ Change the [flight mode](modes.md) to arbitrary one:
|
||||
```python
|
||||
from mavros_msgs.srv import SetMode
|
||||
|
||||
# ...
|
||||
|
||||
set_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
|
||||
|
||||
# ...
|
||||
@@ -297,8 +287,6 @@ Flip:
|
||||
```python
|
||||
import math
|
||||
|
||||
# ...
|
||||
|
||||
PI_2 = math.pi / 2
|
||||
|
||||
def flip():
|
||||
@@ -337,8 +325,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():
|
||||
@@ -372,8 +358,6 @@ Enable and disable [ArUco markers recognition](aruco_marker.md) dynamically (for
|
||||
import rospy
|
||||
import dynamic_reconfigure.client
|
||||
|
||||
# ...
|
||||
|
||||
client = dynamic_reconfigure.client.Client('aruco_detect')
|
||||
|
||||
# Turn markers recognition off
|
||||
@@ -392,8 +376,6 @@ Wait for global position to appear (finishing [GPS receiver](gps.md) initializat
|
||||
```python
|
||||
import math
|
||||
|
||||
# ...
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
if math.isfinite(get_telemetry().lat):
|
||||
break
|
||||
@@ -408,12 +390,8 @@ Read flight controller's parameter:
|
||||
from mavros_msgs.srv import ParamGet
|
||||
from mavros_msgs.msg import ParamValue
|
||||
|
||||
# ...
|
||||
|
||||
param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
|
||||
|
||||
# ...
|
||||
|
||||
# Read parameter of type INT
|
||||
value = param_get(param_id='COM_FLTMODE1').value.integer
|
||||
|
||||
@@ -429,12 +407,8 @@ Set flight controller's parameter:
|
||||
from mavros_msgs.srv import ParamSet
|
||||
from mavros_msgs.msg import ParamValue
|
||||
|
||||
# ...
|
||||
|
||||
param_set = rospy.ServiceProxy('mavros/param/set', ParamSet)
|
||||
|
||||
# ...
|
||||
|
||||
# Set parameter of type INT:
|
||||
param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8))
|
||||
|
||||
|
||||
@@ -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))
|
||||
|
||||
|
||||
Reference in New Issue
Block a user