From 5a98e1ab7822848d7ca4bbc6f96b80718a848307 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 21 Dec 2022 20:50:43 +0300 Subject: [PATCH] Remove yaw_rate from navigate and use set_yaw in examples --- clover/examples/navigate_wait.py | 7 ++----- clover/src/autotest/autotest_aruco.py | 7 ++----- clover/src/autotest/autotest_flight.py | 17 ++++++++--------- 3 files changed, 12 insertions(+), 19 deletions(-) diff --git a/clover/examples/navigate_wait.py b/clover/examples/navigate_wait.py index 99637caf..acb37f70 100644 --- a/clover/examples/navigate_wait.py +++ b/clover/examples/navigate_wait.py @@ -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 diff --git a/clover/src/autotest/autotest_aruco.py b/clover/src/autotest/autotest_aruco.py index d53c98ca..0fbe6fa7 100755 --- a/clover/src/autotest/autotest_aruco.py +++ b/clover/src/autotest/autotest_aruco.py @@ -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 diff --git a/clover/src/autotest/autotest_flight.py b/clover/src/autotest/autotest_flight.py index 1f6c4e4f..7766c792 100755 --- a/clover/src/autotest/autotest_flight.py +++ b/clover/src/autotest/autotest_flight.py @@ -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')