From 99b03ae5bebd6086bfa44edf22279961f48cb7e6 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 19 Oct 2018 03:22:49 +0300 Subject: [PATCH] selfcheck: add rangefinder data checking --- clever/src/selfcheck.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 470e887d..549e44f6 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 sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu from mavros_msgs.msg import State +from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range from geometry_msgs.msg import PoseStamped, TwistStamped @@ -147,6 +147,19 @@ def check_global_position(): failure('No global position') +@check('Rangefinder') +def check_rangefinder(): + # TODO: check FPS! + try: + rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5) + except rospy.ROSException: + failure('No randefinder data from Raspberry') + try: + rospy.wait_for_message('mavros/distance_sensor/rangefinder_0', Range, timeout=0.5) + except rospy.ROSException: + failure('No rangefinder data from PX4') + + @check('Boot duration') def check_boot_duration(): proc = Popen('systemd-analyze', stdout=PIPE) @@ -185,6 +198,7 @@ def selfcheck(): check_camera('main_camera') check_aruco() check_simpleoffboard() + check_rangefinder() check_cpu_usage() check_boot_duration()