CLEVER FLIGHTS

clever-flip.py

#!/usr/bin/python

import math
import rospy
from clever import srv
from time import sleep
from mavros_msgs.srv import SetMode

rospy.init_node('Clever3_Flip')

# creating proxy service
set_rates = rospy.ServiceProxy('/mavros/set_rates', SetRates)
get_telemetry = rospy.ServiceProxy('/get_telemetry', srv.GetTelemetry)

def flip():
    # TODO: use Rate
    start = get_telemetry()

    rospy.loginfo('Flip')
    set_rates(roll_rate=0, thrust=0.8)  # maximum roll rate
    rospy.sleep(0.15)

    print set_rates(roll_rate=5*math.pi, thrust=0.2)  # maximum roll rate
    while True:
        telem = get_telemetry()
        thrust = 0.5 - min(5.6 * abs(telem.roll) / math.pi, 0.4)
        set_rates(roll_rate=5*math.pi, thrust=thrust)  # maximum roll rate
        print math.degrees(telem.roll), '°\t', thrust
        rospy.loginfo('%s ° %s', math.degrees(telem.roll), thrust)
        if abs(telem.roll) > math.pi / 2:
            break
        #if -math.pi / 2 > telem.roll > -math.pi:
        #    break

        rospy.sleep(0.01)

    rospy.loginfo('Stabilize')
    # Finish flip
    set_position(x=start.x, y=start.y, z=start.z, yaw=start.yaw)

clever-square.py

#!/usr/bin/python

#importing
import math
import rospy
from clever import srv
from time import sleep
from mavros_msgs.srv import SetMode

rospy.init_node('Clever3_Flight')

# creating proxy service

navigate = rospy.ServiceProxy('/navigate', srv.Navigate)
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
get_telemetry = rospy.ServiceProxy('/get_telemetry', srv.GetTelemetry)

#functions

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

def height_adjust (zp, sp = 1, tolerance = 0.2):
	start = get_telemetry()
	print navigate(z=zp, speed=sp, frame_id='fcu_horiz', auto_arm=True)
	while True:
		telem = get_telemetry()
		delta = abs(abs(telem.z - start.z)-zp)
		#print delta
		if delta < tolerance:
			break
		rospy.sleep(0.2)

def flight_to_point(xp, yp, zp, sp = 1, tolerance = 0.2, constant_yaw = True):
	frame_id = 'aruco_map'
	if constant_yaw:
		current_yaw = get_telemetry(frame_id = 'aruco_map').yaw
		print navigate(frame_id=frame_id, x=xp, y=yp, z=zp, speed=sp, yaw = current_yaw)
	else:
		print navigate(frame_id=frame_id, x=xp, y=yp, z=zp, speed=sp)
	while True:
		telem = get_telemetry(frame_id=frame_id)
		if get_distance(xp, yp, zp, telem.x, telem.y, telem.z) < tolerance:
			break
		rospy.sleep(0.2)
# parameters
speed = 1
z = 2
width = 2
height = 2
x0 = 0.3
y0 = 0.3

# flight program

height_adjust(height) #takeoff
flight_to_point(x0, y0, z, speed)
flight_to_point(x0, y0 + height, z, speed)
flight_to_point(x0 + width, y0 + height, z, speed)
flight_to_point(x0 + width, y0, z, speed)
flight_to_point(x0, y0, z, speed)
height_adjust(-0.1, 1) #landing
set_mode(base_mode=0, custom_mode='AUTO.LAND')