From ab5a268ca5dc1519c9ce49e6de10a0238169133f Mon Sep 17 00:00:00 2001 From: Anthony Lamping Date: Sat, 30 Dec 2017 13:17:01 -0500 Subject: [PATCH] simplify vtol transition check, more log msgs --- .../mavros/mavros_offboard_attctl_test.py | 14 +++++++++---- .../mavros/mavros_offboard_posctl_test.py | 10 ++++----- .../python_src/px4_it/mavros/mission_test.py | 21 ++++--------------- 3 files changed, 19 insertions(+), 26 deletions(-) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py index 175010b540..5d32d0b32c 100755 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py @@ -221,6 +221,10 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # def test_attctl(self): """Test offboard attitude control""" + # boundary to cross + boundary_x = 5 + boundary_y = 5 + boundary_z = -5 # delay starting the mission self.wait_for_topics(30) @@ -231,15 +235,17 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.set_arm(True, 5) rospy.loginfo("run mission") + rospy.loginfo("attempting to cross boundary | x: {0}, y: {1}, z: {2}". + format(boundary_x, boundary_y, boundary_z)) # does it cross expected boundaries in 'timeout' seconds? timeout = 12 # (int) seconds loop_freq = 10 # Hz rate = rospy.Rate(loop_freq) crossed = False for i in xrange(timeout * loop_freq): - if (self.local_position.pose.position.x > 5 and - self.local_position.pose.position.z > 5 and - self.local_position.pose.position.y < -5): + if (self.local_position.pose.position.x > boundary_x and + self.local_position.pose.position.z > boundary_y and + self.local_position.pose.position.y < boundary_z): rospy.loginfo("boundary crossed | seconds: {0} of {1}".format( i / loop_freq, timeout)) crossed = True @@ -248,7 +254,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): rate.sleep() self.assertTrue(crossed, ( - "took too long to cross boundaries | x: {0}, y: {1}, z: {2} | timeout(seconds): {3}". + "took too long to cross boundaries | current position x: {0}, y: {1}, z: {2} | timeout(seconds): {3}". format(self.local_position.pose.position.x, self.local_position.pose.position.y, self.local_position.pose.position.z, timeout))) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py index 069b11f507..00da68df11 100755 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py @@ -231,6 +231,8 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.pos.pose.position.x = x self.pos.pose.position.y = y self.pos.pose.position.z = z + rospy.loginfo("attempting to reach position | x: {0}, y: {1}, z: {2}". + format(x, y, z)) # For demo purposes we will lock yaw/heading to north. yaw_degrees = 0 # North @@ -246,17 +248,15 @@ class MavrosOffboardPosctlTest(unittest.TestCase): if self.is_at_position(self.pos.pose.position.x, self.pos.pose.position.y, self.pos.pose.position.z, 1): - rospy.loginfo( - "position reached | x: {0}, y: {1}, z: {2} | seconds: {3} of {4}". - format(self.pos.pose.position.x, self.pos.pose.position.y, - self.pos.pose.position.z, i / loop_freq, timeout)) + rospy.loginfo("position reached | seconds: {0} of {1}".format( + i / loop_freq, timeout)) reached = True break rate.sleep() self.assertTrue(reached, ( - "took too long to get to position | x: {0}, y: {1}, z: {2} | timeout(seconds): {3}". + "took too long to get to position | current position x: {0}, y: {1}, z: {2} | timeout(seconds): {3}". format(self.local_position.pose.position.x, self.local_position.pose.position.y, self.local_position.pose.position.z, timeout))) diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index 4cacdfd19d..8d69b99b35 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -346,7 +346,7 @@ class MavrosMissionTest(unittest.TestCase): if self.is_at_position(lat, lon, alt, xy_radius, z_radius): reached = True rospy.loginfo( - "position reached | pos_d: {0}, alt_d: {1}, index: {2} | seconds: {3} of {4}". + "position reached | pos_d: {0:.2f}, alt_d: {1:.2f}, index: {2} | seconds: {3} of {4}". format(self.last_pos_d, self.last_alt_d, index, i / loop_freq, timeout)) break @@ -354,7 +354,7 @@ class MavrosMissionTest(unittest.TestCase): rate.sleep() self.assertTrue(reached, ( - "({0}) took too long to get to position | lat: {1:13.9f}, lon: {2:13.9f}, alt: {3:6.2f}, xy off: {4}, z off: {5}, pos_d: {6}, alt_d: {7}, VTOL state: {8}, index: {9} | timeout(seconds): {10}". + "({0}) took too long to get to position | lat: {1:13.9f}, lon: {2:13.9f}, alt: {3:6.2f}, xy off: {4}, z off: {5}, pos_d: {6:.2f}, alt_d: {7:.2f}, VTOL state: {8}, index: {9} | timeout(seconds): {10}". format(self.mission_name, lat, lon, alt, xy_radius, z_radius, self.last_pos_d, self.last_alt_d, self.VTOL_STATES.get(self.extended_state.vtol_state), index, @@ -392,7 +392,7 @@ class MavrosMissionTest(unittest.TestCase): if self.extended_state.landed_state == desired_landed_state: landed_state_confirmed = True rospy.loginfo( - "landed state confirmed | state: {0}, index{1}".format( + "landed state confirmed | state: {0}, index: {1}".format( self.LAND_STATES.get(desired_landed_state), index)) break @@ -414,20 +414,7 @@ class MavrosMissionTest(unittest.TestCase): rate = rospy.Rate(loop_freq) transitioned = False for i in xrange(timeout * loop_freq): - # transition to MC - if (transition == ExtendedState.VTOL_STATE_MC and - self.extended_state.vtol_state == - ExtendedState.VTOL_STATE_MC): - rospy.loginfo( - "transitioned | index: {0} | seconds: {1} of {2}".format( - index, i / loop_freq, timeout)) - transitioned = True - break - - # transition to FW - if (transition == ExtendedState.VTOL_STATE_FW and - self.extended_state.vtol_state == - ExtendedState.VTOL_STATE_FW): + if transition == self.extended_state.vtol_state: rospy.loginfo( "transitioned | index: {0} | seconds: {1} of {2}".format( index, i / loop_freq, timeout))