From 751412e6392f936be43d82951c279e1a1381e7ab Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sat, 22 Sep 2018 01:46:31 +0300
Subject: [PATCH] selfcheck.py: check camera info dimensions

---
 clever/src/selfcheck.py | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py
index c5c47746..456dc625 100755
--- a/clever/src/selfcheck.py
+++ b/clever/src/selfcheck.py
@@ -63,16 +63,21 @@ def check_fcu():
 @check('Camera')
 def check_camera(name):
     try:
-        rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
+        img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
     except rospy.ROSException:
-        failure('%s: No images', name)
+        failure('%s: No images (is the camera connected properly?)', name)
         return
     try:
-        rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=3)
+        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
     except rospy.ROSException:
         failure('%s: No calibration info', name)
         return
 
+    if img.width != info.width:
+        failure('%s: Calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
+    if img.height != info.height:
+        failure('%s: Calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
+
 
 @check('Aruco detector')
 def check_aruco():