From 90049182cfde251339dd7c1a943c97e6cbd138d5 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 9 Apr 2020 15:44:48 +0300 Subject: [PATCH] image: add navigate_wait example --- builder/assets/examples/navigate_wait.py | 41 ++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 builder/assets/examples/navigate_wait.py diff --git a/builder/assets/examples/navigate_wait.py b/builder/assets/examples/navigate_wait.py new file mode 100644 index 00000000..625c1535 --- /dev/null +++ b/builder/assets/examples/navigate_wait.py @@ -0,0 +1,41 @@ +# Information: https://clever.coex.tech/en/snippets.html#block-nav + +import math +import rospy +from clover import srv +from std_srvs.srv import Trigger + +rospy.init_node('flight') + +get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) +navigate = rospy.ServiceProxy('navigate', srv.Navigate) +navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal) +set_position = rospy.ServiceProxy('set_position', srv.SetPosition) +set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity) +set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) +set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) +land = rospy.ServiceProxy('land', Trigger) + +def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \ + frame_id='body', tolerance=0.2, auto_arm=False): + + res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \ + frame_id=frame_id, auto_arm=auto_arm) + + if not res.success: + return res + + 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: + return res + rospy.sleep(0.2) + +# Take off 1 meter +navigate_wait(z=1, frame_id='body', auto_arm=True) + +# Fly forward 1 m +navigate_wait(x=1, frame_id='body') + +# Land +land()