diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 27dd31bc..d2ea13a0 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -8,12 +8,14 @@ from threading import Event import numpy import rospy from systemd import journal +import tf2_ros +import tf2_geometry_msgs from pymavlink import mavutil from std_srvs.srv import Trigger from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range from mavros_msgs.msg import State, OpticalFlowRad, Mavlink from mavros_msgs.srv import ParamGet -from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped +from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped import tf.transformations as t from aruco_pose.msg import MarkerArray from mavros import mavlink @@ -31,6 +33,10 @@ from mavros import mavlink rospy.init_node('selfcheck') +tf_buffer = tf2_ros.Buffer() +tf_listener = tf2_ros.TransformListener(tf_buffer) + + failures = [] @@ -169,6 +175,23 @@ def check_fcu(): failure('no MAVROS state (check wiring)') +def describe_direction(v): + if v.x > 0.9: + return 'forward' + elif v.x < - 0.9: + return 'backward' + elif v.y > 0.9: + return 'left' + elif v.y < -0.9: + return 'right' + elif v.z > 0.9: + return 'upward' + elif v.z < -0.9: + return 'downward' + else: + return None + + def check_camera(name): try: img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1) @@ -186,6 +209,24 @@ def check_camera(name): if img.height != info.height: failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height) + try: + optical = Vector3Stamped() + optical.header.frame_id = img.header.frame_id + optical.vector.z = 1 + cable = Vector3Stamped() + cable.header.frame_id = img.header.frame_id + cable.vector.y = 1 + + optical = describe_direction(tf_buffer.transform(optical, 'base_link').vector) + cable = describe_direction(tf_buffer.transform(cable, 'base_link').vector) + if not optical or not cable: + rospy.loginfo('%s: custom camera orientation detected', name) + else: + rospy.loginfo('%s is oriented %s, camera cable goes %s', name, optical, cable) + + except tf2_ros.TransformException: + failure('cannot transform from base_link to camera frame') + @check('Main camera') def check_main_camera():