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 5d724b4c9d..7c81e75c90 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 @@ -132,7 +132,10 @@ class MavrosOffboardAttctlTest(MavrosTestCommon): crossed = True break - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(crossed, ( "took too long to cross boundaries | current position x: {0:.2f}, y: {1:.2f}, z: {2:.2f} | timeout(seconds): {3}". 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 263f1597be..6778371905 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 @@ -139,7 +139,10 @@ class MavrosOffboardPosctlTest(MavrosTestCommon): reached = True break - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(reached, ( "took too long to get to position | current position x: {0:.2f}, y: {1:.2f}, z: {2:.2f} | timeout(seconds): {3}". diff --git a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py index 810299a20c..b9d9c9ffa7 100644 --- a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py @@ -48,7 +48,7 @@ class MavrosTestCommon(unittest.TestCase): except rospy.ROSException: self.fail("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) - self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming', + self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', @@ -184,7 +184,10 @@ class MavrosTestCommon(unittest.TestCase): except rospy.ServiceException as e: rospy.logerr(e) - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(arm_set, ( "failed to set arm | new arm: {0}, old arm: {1} | timeout(seconds): {2}". @@ -211,7 +214,10 @@ class MavrosTestCommon(unittest.TestCase): except rospy.ServiceException as e: rospy.logerr(e) - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(mode_set, ( "failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}". @@ -232,7 +238,10 @@ class MavrosTestCommon(unittest.TestCase): format(i / loop_freq, timeout)) break - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(simulation_ready, ( "failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}". @@ -252,13 +261,16 @@ class MavrosTestCommon(unittest.TestCase): format(i / loop_freq, timeout)) break - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(landed_state_confirmed, ( "landed state not detected | desired: {0}, current: {1} | index: {2}, timeout(seconds): {3}". format(mavutil.mavlink.enums['MAV_LANDED_STATE'][ desired_landed_state].name, mavutil.mavlink.enums[ - 'MAV_VTOL_STATE'][self.extended_state.landed_state].name, + 'MAV_LANDED_STATE'][self.extended_state.landed_state].name, index, timeout))) def wait_for_vtol_state(self, transition, timeout, index): @@ -277,7 +289,10 @@ class MavrosTestCommon(unittest.TestCase): transitioned = True break - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(transitioned, ( "transition not detected | desired: {0}, current: {1} | index: {2} timeout(seconds): {3}". @@ -304,7 +319,10 @@ class MavrosTestCommon(unittest.TestCase): except rospy.ServiceException as e: rospy.logerr(e) - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(wps_cleared, ( "failed to clear waypoints | timeout(seconds): {0}".format(timeout) @@ -340,7 +358,10 @@ class MavrosTestCommon(unittest.TestCase): format(i / loop_freq, timeout)) break - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(( wps_sent and wps_verified @@ -366,7 +387,10 @@ class MavrosTestCommon(unittest.TestCase): except rospy.ServiceException as e: rospy.logerr(e) - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue(res.success, ( "MAV_TYPE param get failed | timeout(seconds): {0}".format(timeout) diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index 5de036f6ea..a048c6134d 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -252,7 +252,10 @@ class MavrosMissionTest(MavrosTestCommon): format(self.mission_wp.current_seq, xy_radius, z_radius, pos_xy_d, pos_z_d)) - rate.sleep() + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) self.assertTrue( reached, diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch index 7bc706a87a..390452cd86 100644 --- a/launch/mavros_posix_sitl.launch +++ b/launch/mavros_posix_sitl.launch @@ -24,6 +24,7 @@ + @@ -45,6 +46,7 @@ + diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch index d06f6b8a54..9e828182c2 100644 --- a/launch/posix_sitl.launch +++ b/launch/posix_sitl.launch @@ -20,6 +20,7 @@ + @@ -32,6 +33,7 @@ + + @@ -14,6 +15,7 @@ + diff --git a/test/mavros_posix_tests_iris_opt_flow.test b/test/mavros_posix_tests_iris_opt_flow.test index f4361e1285..ae174029fd 100644 --- a/test/mavros_posix_tests_iris_opt_flow.test +++ b/test/mavros_posix_tests_iris_opt_flow.test @@ -5,6 +5,7 @@ + @@ -12,6 +13,7 @@ + diff --git a/test/mavros_posix_tests_missions.test b/test/mavros_posix_tests_missions.test index 2494d93b89..2ac8217ad4 100644 --- a/test/mavros_posix_tests_missions.test +++ b/test/mavros_posix_tests_missions.test @@ -5,6 +5,7 @@ + @@ -12,6 +13,7 @@ + diff --git a/test/mavros_posix_tests_offboard_attctl.test b/test/mavros_posix_tests_offboard_attctl.test index 890a36ad17..e6407166f1 100644 --- a/test/mavros_posix_tests_offboard_attctl.test +++ b/test/mavros_posix_tests_offboard_attctl.test @@ -5,6 +5,7 @@ + @@ -12,6 +13,7 @@ + diff --git a/test/mavros_posix_tests_offboard_posctl.test b/test/mavros_posix_tests_offboard_posctl.test index 38d5b06566..92905e6870 100644 --- a/test/mavros_posix_tests_offboard_posctl.test +++ b/test/mavros_posix_tests_offboard_posctl.test @@ -5,6 +5,7 @@ + @@ -12,6 +13,7 @@ +