mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-31 23:19:32 +00:00
39 lines
1.3 KiB
Python
39 lines
1.3 KiB
Python
# Information: https://clovercoex.tech/en/snippets.html#navigate_wait
|
|
|
|
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=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
|
|
res = navigate(x=x, y=y, z=z, yaw=yaw, 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)
|
|
|
|
print('Take off 1 meter')
|
|
navigate_wait(z=1, frame_id='body', auto_arm=True)
|
|
|
|
print('Fly forward 1 m')
|
|
navigate_wait(x=1, frame_id='body')
|
|
|
|
print('Land')
|
|
land()
|