simplify vtol transition check, more log msgs

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)))

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)))

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))