CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove yaw_rate from navigate and use set_yaw in examples

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-21 20:50:43 +03:00
parent 84b085352e
commit 5a98e1ab78
3 changed files with 12 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
clover/examples/navigate_wait.py
View File

@@ -16,11 +16,8 @@ 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)
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

7
clover/src/autotest/autotest_aruco.py
View File

@@ -35,11 +35,8 @@ def print_current_map_position():
dist = rospy.wait_for_message('rangefinder/range', Range).range
print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
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)
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

17
clover/src/autotest/autotest_flight.py
View File

@@ -15,6 +15,8 @@ rospy.init_node('autotest_flight', disable_signals=True) # disable signals to al
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
set_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw))
set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate))
set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
@@ -28,11 +30,8 @@ def interrupt(sig, frame):
signal.signal(signal.SIGINT, interrupt)
def navigate_wait(x=0, y=0, z=0, yaw=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)
def navigate_wait(x=0, y=0, z=0, yaw=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
@@ -69,8 +68,8 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
rospy.sleep(2)
set_position(frame_id='body')
input('Rotate right 90° [enter] ')
navigate(yaw=-math.pi / 2, frame_id='navigate_target')
input('Rotate right 90° using set_yaw [enter] ')
set_yaw(yaw=-math.pi / 2, frame_id='navigate_target')
rospy.sleep(3)
input('Use set_attitude to fly backwards [enter]')
@@ -88,8 +87,8 @@ set_rates(roll_rate=1.2, thrust=0.5)
rospy.sleep(0.4)
set_position(frame_id='body')
input('Rotate 360° to the right using yaw_rate [enter]')
set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
input('Rotate 360° to the right using set_yaw_rate [enter]')
set_yaw_rate(yaw_rate=-1)
rospy.sleep(2 * math.pi)
set_position(frame_id='body')