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

selfcheck.py: don't failure when no vpe and drone is on the floor

Browse Source
This commit is contained in:
Oleg Kalachev
2022-10-30 19:21:42 +06:00
parent 6361984794
commit 1700ad24df
1 changed files with 14 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
clover/src/selfcheck.py
View File

@@ -396,6 +396,14 @@ def check_aruco():
info('aruco_map is not running')
def is_on_the_floor():
try:
dist = rospy.wait_for_message('rangefinder/range', Range, timeout=1)
return dist.range < 0.3
except rospy.ROSException:
return False
@check('Vision position estimate')
def check_vpe():
vis = None
@@ -405,7 +413,12 @@ def check_vpe():
try:
vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
except rospy.ROSException:
failure('no VPE or MoCap messages')
if rospy.get_param('aruco_map/known_tilt') == 'map_flipped':
failure('no vision position estimate, markers are on the ceiling')
elif is_on_the_floor():
info('no vision position estimate, the drone is on the floor')
else:
failure('no vision position estimate')
# check if vpe_publisher is running
try:
subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])