CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

selfcheck: add vpe or mocap data checking

Browse Source
This commit is contained in:
Oleg Kalachev
2018-10-19 03:32:42 +03:00
parent 802d04e1eb
commit d376bc0553
1 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
clever/src/selfcheck.py
View File

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