Add experimental selfcheck node

clever/src/selfcheck.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+import rospy
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu
+from mavros_msgs.msg import State
+from geometry_msgs.msg import PoseStamped
+
+
+# TODO: roscore is running
+# TODO: local_origin, fcu, fcu_horiz
+# TODO: rc service
+
+rospy.init_node('selfcheck')
+
+
+def check_fcu():
+    try:
+        state = rospy.wait_for_message('mavros/state', State, timeout=3)
+        if not state.connected:
+            raise Exception('No connection to the FCU')
+    except:
+        raise Exception('No mavros state')
+
+
+def check_camera(name):
+    try:
+        rospy.wait_for_message(name + '/image_raw', Image, timeout=3)
+    except:
+        raise Exception('No %s camera images' % name)
+    try:
+        rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=3)
+    except:
+        raise Exception('No %s camera camera info' % name)
+
+def check_aruco():
+    try:
+        rospy.wait_for_message('aruco_pose/debug', Image, timeout=3)
+    except:
+        raise Exception('No aruco_pose/debug topic')
+
+
+def check_simpleoffboard():
+    try:
+        rospy.wait_for_service('navigate', timeout=3)
+        rospy.wait_for_service('get_telemetry', timeout=3)
+        rospy.wait_for_service('land', timeout=3)
+    except:
+        raise Exception('No simple_offboard services')
+
+
+def check_imu():
+    try:
+        rospy.wait_for_message('mavros/imu/data', Imu, timeout=3)
+    except:
+        raise Exception('No IMU data')
+
+
+def check_local_position():
+    try:
+        rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=3)
+    except:
+        raise Exception('No local position')
+
+
+def check_global_position():
+    try:
+        rospy.wait_for_message('mavros/global_position/global', PoseStamped, timeout=3)
+    except:
+        raise Exception('No global position')
+
+
+def check(name, fn):
+    try:
+        fn()
+        rospy.loginfo('%s: OK', name)
+    except Exception as e:
+        rospy.logwarn('%s: %s', name, str(e))
+
+
+def selfcheck():
+    check('FCU', check_fcu)
+    check('Simple offboard node', check_simpleoffboard)
+    check('Main camera node', lambda: check_camera('main_camera'))
+    check('aruco_pose/debug topic', check_aruco)
+    check('IMU data', check_imu)
+    check('Local position', check_local_position)
+    check('Global position (GPS)', check_global_position)
+
+
+if __name__ == '__main__':
+    rospy.loginfo('Performing selfcheck...')
+    selfcheck()