From 515777bd9637b82e0e98007cde6d74a5b740788c Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Thu, 5 Apr 2018 03:31:30 +0300
Subject: [PATCH] Added pexample how to run program with remote control

---
 docs/snippets.md | 138 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 138 insertions(+)

diff --git a/docs/snippets.md b/docs/snippets.md
index d5c25fb2..b6702402 100644
--- a/docs/snippets.md
+++ b/docs/snippets.md
@@ -57,6 +57,144 @@ while True:
 
 ---
 
+Запуск полётной программы с пульта:
+
+```python
+
+#!/usr/bin/python
+import rospy
+import thread
+import sys
+import math
+from mavros_msgs.msg import RCIn
+from clever import srv
+from time import sleep
+from mavros_msgs.srv import SetMode
+from mavros_msgs.srv import CommandBool
+
+states = ('start','stop','unknown')
+state = states[2]
+
+rospy.init_node('Clever3_RC_Script')
+
+navigate = rospy.ServiceProxy('/navigate', srv.Navigate)
+set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
+get_telemetry = rospy.ServiceProxy('/get_telemetry', srv.GetTelemetry)
+arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
+
+def get_distance(x1, y1, z1, x2, y2, z2):
+	return math.sqrt((x1-x2)**2 + (y1-y2)**2 +(z1-z2)**2)
+
+def takeoff (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)
+		if delta < tolerance:
+			break
+		rospy.sleep(0.2)
+
+def land(sp = 1, land_height = -1, tolerance = 0.25):
+	print 'land!'
+	z0 = get_telemetry(frame_id='local_origin').z
+	print z0	
+	h = get_telemetry(frame_id='aruco_map').z
+	print h
+	print navigate(z=-h+land_height, speed=sp, frame_id='fcu_horiz')
+	while True:
+		z = get_telemetry(frame_id='local_origin').z
+		delta = z0-z-h
+		print delta
+		if (abs(delta) < tolerance):
+			print get_telemetry(frame_id='local_origin')			
+			arming(False)
+			break
+		rospy.sleep(0.2)			
+
+def flight_to_point(xp, yp, zp, sp = 1, breakable = True, 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:
+		if breakable and state == 'stop':
+			return
+		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)
+
+# copter parameters
+
+speed = 1
+z = 1
+
+# rectangle parameters
+
+width = 1
+height = 1
+x0 = 0.1
+y0 = 0.6
+
+# flight program
+
+def flight_program (param):
+	while True:
+
+		global state
+		print 'waiting for stop!'
+		while state != 'stop':
+		    rospy.sleep(0.1)
+		print 'waiting for start...'
+		while state == 'stop':
+		    rospy.sleep(0.1)
+		print 'start!'
+
+		takeoff(z) #takeoff
+		flight_to_point(x0, y0, z, speed)
+		while True:
+			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, breakable = False)
+			if state == 'stop':
+				break
+		land()
+
+# Вызывается при обновлении данных из топика
+def callback(data):
+	global state
+	# Обрабатываем данные с 6 канала пульта
+	if data.channels[5] < 1100:
+		state = states[1]
+	elif data.channels[5] > 1900:
+		state = states[0]
+	else:
+		state = states[2]	
+
+def listener():
+
+    # In ROS, nodes are uniquely named. If two nodes with the same
+    # name are launched, the previous one is kicked off. The
+    # anonymous=True flag means that rospy will choose a unique
+    # name for our 'listener' node so that multiple listeners can
+    # run simultaneously.
+
+	rospy.Subscriber('mavros/rc/in', RCIn, callback)
+
+    # spin() simply keeps python from exiting until this node is stopped
+	
+	rospy.spin()
+
+param = []
+thread.start_new_thread(flight_program, (param,))
+listener()
+```
+---
+
 Флип:
 
 TODO