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.py: check PX4 VPE parameters when vpe_publisher is running

Browse Source
This commit is contained in:
Oleg Kalachev
2019-05-11 10:12:38 +03:00
parent 747f26742d
commit 34095bfaa7
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
clever/src/selfcheck.py
View File

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