# Code examples

## Python

### #

Calculating the distance between two points (important: the points are to be in the same system of coordinates):

``````def get_distance(x1, y1, z1, x2, y2, z2):
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
``````

### #

Approximation of distance (in meters) between two global coordinates (latitude/longitude):

``````def get_distance_global(lat1, lon1, lat2, lon2):
return math.hypot(lat1 - lat2, lon1 - lon2) * 1.113195e5
``````

### #

Takeoff and waiting for it to finish:

``````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:

``````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)
``````

### #

Flying towards a point and waiting for copter's arrival:

``````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:

``````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:

``````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)
``````

This code also can be used for navigating using `body` frame.

### #

Landing and waiting until the copter lands:

``````land()
while get_telemetry().armed:
rospy.sleep(0.2)
``````

This code can be wrapped in a function:

``````def land_wait():
land()
while get_telemetry().armed:
rospy.sleep(0.2)
``````

### #

Quadcopter disarm (disabling propellers the copter will fall down):

``````# Declaring a proxy:
from mavros_msgs.srv import CommandBool
arming = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)

# ...

arming(False)  # дизарм
``````

### #

Transforming the position (`PoseStamped`) from one system of coordinates (of frame) to another one using [tf2] (http://wiki.ros.org/tf2):

``````import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import PoseStamped

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)

# ...

# PoseStamped object creation (or getting it from a topic):
pose = PoseStamped()
pose.header.frame_id = 'map' # frame, which is the position is specified
pose.header.stamp = rospy.get_rostime() # the instant for which the position is specified (current time)
pose.pose.position.x = 1
pose.pose.position.y = 2
pose.pose.position.z = 3
pose.pose.orientation.w = 1

frame_id = 'base_link' # target frame
transform_timeout = rospy.Duration(0.2) # wait for transformation timeout

# Transforming the position from the old frame to the new one:
new_pose = tf_buffer.transform(pose, frame_id, transform_timeout)
``````

### #

Determining whether the copter is turned upside-down:

``````PI_2 = math.pi / 2
telem = get_telemetry()

flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
``````

### #

Calculating the copter total horizontal angle:

``````PI_2 = math.pi / 2
telem = get_telemetry()

flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
if flipped:
angle_to_horizon = math.pi - angle_to_horizon
``````

### #

Flying along a circular path:

``````RADIUS = 0.6  # m
SPEED = 0.3 # rad / s

start = get_telemetry()
start_stamp = rospy.get_rostime()

r = rospy.Rate(10)

while not rospy.is_shutdown():
angle = (rospy.get_rostime() - start_stamp).to_sec() * SPEED
x = start.x + math.sin(angle) * RADIUS
y = start.y + math.cos(angle) * RADIUS
set_position(x=x, y=y, z=start.z)

r.sleep()
``````

### #

repeating an action at a frequency of 10 Hz:

``````r = rospy.Rate(10)
while not rospy.is_shutdown():
# Do anything
r.sleep()
``````

### #

An example of subscription to a topic from MAVROS

``````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)
``````

Sending an arbitrary MAVLink message to the copter:

``````# ...

# ...

# Sending a HEARTBEAT message:

``````

### #

Return on mode switching with the transmitter (may be used for starting an autonomous flight, see example):

``````from mavros_msgs.msg import RCIn

# Called when new data is received from the transmitter
def rc_callback(data):
# Return on switch toggling of the transmitter
if data.channels < 1100:
# ...
pass
elif data.channels > 1900:
# ...
pass
else:
# ...
pass

# Creating a subscriber for the topic with the data from the transmitter
rospy.Subscriber('mavros/rc/in', RCIn, rc_callback)

rospy.spin()
``````

### #

Change the flight mode to arbitrary one:

``````from mavros_msgs.srv import SetMode

# ...

set_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)

# ...

set_mode(custom_mode='STABILIZED')
``````

### #

Flip:

``````import math

# ...

PI_2 = math.pi / 2

def flip():
start = get_telemetry()  # memorize starting position

set_rates(thrust=1)  # bump up
rospy.sleep(0.2)

set_rates(pitch_rate=30, thrust=0.2)  # pitch flip
# set_rates(roll_rate=30, thrust=0.2)  # roll flip

while True:
telem = get_telemetry()
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
if flipped:
break

set_position(x=start.x, y=start.y, z=start.z, yaw=start.yaw)  # finish flip

print navigate(z=2, speed=1, frame_id='body', auto_arm=True)  # take off
rospy.sleep(10)

flip()
``````

Requires the special PX4 firmware for Clever. Before running a flip, take all necessary safty precautions.

### #

Perform gyro calibration:

``````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():
return False

calibrating = False
while not rospy.is_shutdown():
state = rospy.wait_for_message('mavros/state', State)