From d376bc0553c0e09a6772385e3b26d5152f10cce6 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 19 Oct 2018 03:32:42 +0300 Subject: [PATCH] selfcheck: add vpe or mocap data checking --- clever/src/selfcheck.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index a3ed98bf..314cc095 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -87,6 +87,17 @@ def check_aruco(): failure('No aruco_pose/debug messages') +@check('Visual position estimate') +def check_vpe(): + try: + rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1) + except rospy.ROSException: + try: + rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1) + except rospy.ROSException: + failure('No VPE or MoCap messages') + + @check('Simple offboard node') def check_simpleoffboard(): try: @@ -142,7 +153,7 @@ def check_velocity(): @check('Global position (GPS)') def check_global_position(): try: - rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=2) + rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1) except rospy.ROSException: failure('No global position') @@ -208,6 +219,7 @@ def selfcheck(): check_aruco() check_simpleoffboard() check_optical_flow() + check_vpe() check_rangefinder() check_cpu_usage() check_boot_duration()