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: support flip_vertical parameter

Browse Source
This commit is contained in:
Oleg Kalachev
2022-11-11 05:58:09 +06:00
parent c3625490b2
commit 8fd69e4ea5
1 changed files with 14 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
clover/src/selfcheck.py
View File

@@ -402,11 +402,13 @@ def check_aruco():
except KeyError:
failure('aruco_detect/length parameter is not set')
known_vertical = rospy.get_param('aruco_detect/known_vertical', '')
if known_vertical == 'map':
known_vertical += ' (ALL markers are on the floor)'
elif known_vertical == 'map_flipped':
known_vertical += ' (ALL markers are on the ceiling)'
flip_vertical = str(rospy.get_param('aruco_detect/flip_vertical', False))
if known_vertical == 'map' and not flip_vertical:
flip_vertical += ' (all markers are on the floor)'
elif known_vertical == 'map' and flip_vertical:
flip_vertical += ' (all markers are on the ceiling)'
info('aruco_detect/known_vertical = %s', known_vertical)
info('aruco_detect/flip_vertical = %s', flip_vertical)
try:
markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
except rospy.ROSException:
@@ -418,11 +420,13 @@ def check_aruco():
if is_process_running('aruco_map', full=True):
known_vertical = rospy.get_param('aruco_map/known_vertical', '')
if known_vertical == 'map':
known_vertical += ' (marker\'s map is on the floor)'
elif known_vertical == 'map_flipped':
known_vertical += ' (marker\'s map is on the ceiling)'
flip_vertical = str(rospy.get_param('aruco_map/flip_vertical', False))
if known_vertical == 'map' and not flip_vertical:
flip_vertical += ' (markers map is on the floor)'
elif known_vertical == 'map' and flip_vertical:
flip_vertical += ' (markers map is on the ceiling)'
info('aruco_map/known_vertical = %s', known_vertical)
info('aruco_map/flip_vertical = %s', flip_vertical)
try:
visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8)
@@ -462,7 +466,8 @@ def check_vpe():
except rospy.ROSException:
if not is_process_running('vpe_publisher', full=True):
info('no vision position estimate, vpe_publisher is not running')
elif rospy.get_param('aruco_map/known_vertical', '') == 'map_flipped':
elif rospy.get_param('aruco_map/known_vertical', '') == 'map' \
and rospy.get_param('aruco_map/flip_vertical', False):
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')