From 4f114184bff820be7ee71ece9e94e8825e6ed114 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 17 Aug 2018 18:03:51 +0300
Subject: [PATCH] Improvements to selfcheck node

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

diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py
index 570464a9..b7c508b5 100755
--- a/clever/src/selfcheck.py
+++ b/clever/src/selfcheck.py
@@ -4,7 +4,7 @@ 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
+from geometry_msgs.msg import PoseStamped, TwistStamped
 
 
 # TODO: roscore is running
@@ -21,24 +21,24 @@ def check_fcu():
         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')
+    except rospy.ROSException:
+        raise Exception('No MAVROS state')
 
 
 def check_camera(name):
     try:
         rospy.wait_for_message(name + '/image_raw', Image, timeout=3)
-    except:
+    except rospy.ROSException:
         raise Exception('No %s camera images' % name)
     try:
         rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=3)
-    except:
+    except rospy.ROSException:
         raise Exception('No %s camera camera info' % name)
 
 def check_aruco():
     try:
         rospy.wait_for_message('aruco_pose/debug', Image, timeout=3)
-    except:
+    except rospy.ROSException:
         raise Exception('No aruco_pose/debug topic')
 
 
@@ -47,28 +47,41 @@ def check_simpleoffboard():
         rospy.wait_for_service('navigate', timeout=3)
         rospy.wait_for_service('get_telemetry', timeout=3)
         rospy.wait_for_service('land', timeout=3)
-    except:
+    except rospy.ROSException:
         raise Exception('No simple_offboard services')
 
 
 def check_imu():
     try:
         rospy.wait_for_message('mavros/imu/data', Imu, timeout=3)
-    except:
+    except rospy.ROSException:
         raise Exception('No IMU data')
 
 
 def check_local_position():
     try:
         rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=3)
-    except:
+    except rospy.ROSException:
         raise Exception('No local position')
 
 
+def check_velocity():
+    try:
+        velocity = rospy.wait_for_message('mavros/local_position/velocity', TwistStamped, timeout=3)
+        horiz = math.hypot(velocity.twist.linear.x, velocity.twist.linear.y)
+        vert = velocity.twist.linear.z
+        if abs(horiz) > 0.1:
+            raise Exception('Horizontal velocity estimation is %s m/s; is the copter staying still?' % horiz)
+        if abs(vert) > 0.1:
+            raise Exception('Vertical velocity estimation is %s m/s; is the copter staying still?' % vert)
+    except rospy.ROSException:
+        raise Exception('No velocity estimation')
+
+
 def check_global_position():
     try:
         rospy.wait_for_message('mavros/global_position/global', PoseStamped, timeout=3)
-    except:
+    except rospy.ROSException:
         raise Exception('No global position')
 
 
@@ -76,7 +89,7 @@ def check(name, fn):
     try:
         fn()
         rospy.loginfo('%s: OK', name)
-    except Exception as e:
+    except BaseException as e:
         rospy.logwarn('%s: %s', name, str(e))
 
 
@@ -87,6 +100,7 @@ def selfcheck():
     check('aruco_pose/debug topic', check_aruco)
     check('IMU data', check_imu)
     check('Local position', check_local_position)
+    check('Velocity estimation', check_velocity)
     check('Global position (GPS)', check_global_position)