diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 01f0df65..ee5b6353 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -131,6 +131,7 @@ def check_aruco(): @check('Vision position estimate') def check_vpe(): + vis = None try: vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1) except rospy.ROSException: @@ -138,7 +139,11 @@ def check_vpe(): vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1) except rospy.ROSException: failure('no VPE or MoCap messages') - return + # check if vpe_publisher is running + try: + subprocess.check_output(['pgrep', '-x', 'vpe_publisher']) + except subprocess.CalledProcessError: + return # it's not running, skip following checks # check PX4 settings est = get_param('SYS_MC_EST_GROUP') @@ -171,6 +176,9 @@ def check_vpe(): get_param('EKF2_EVA_NOISE'), get_param('EKF2_EVP_NOISE')) + if not vis: + return + # check vision pose and estimated pose inconsistency try: pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)