From 802d04e1eb8ecfe08c7c9fcc9c393d4c56d34d43 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 19 Oct 2018 03:23:46 +0300 Subject: [PATCH] selfcheck: add checking optical flow data --- clever/src/selfcheck.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 549e44f6..a3ed98bf 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -6,8 +6,8 @@ import re import traceback import rospy from std_srvs.srv import Trigger -from mavros_msgs.msg import State from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range +from mavros_msgs.msg import State, OpticalFlowRad from geometry_msgs.msg import PoseStamped, TwistStamped @@ -147,6 +147,15 @@ def check_global_position(): failure('No global position') +@check('Optical flow') +def check_optical_flow(): + # TODO:check FPS! + try: + rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5) + except rospy.ROSException: + failure('No optical flow data (from Raspberry)') + + @check('Rangefinder') def check_rangefinder(): # TODO: check FPS! @@ -198,6 +207,7 @@ def selfcheck(): check_camera('main_camera') check_aruco() check_simpleoffboard() + check_optical_flow() check_rangefinder() check_cpu_usage() check_boot_duration()