From 34095bfaa7786b324e6c497714c42b01ccae0915 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 11 May 2019 10:12:38 +0300 Subject: [PATCH] selfcheck.py: check PX4 VPE parameters when vpe_publisher is running --- clever/src/selfcheck.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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)