From 466954c225aa25e83b48ecb48aecc51780eba65d Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 15 Nov 2019 06:30:09 +0000
Subject: [PATCH] Client: Add visual_pose_watchdog node

---
 Drone/visual_pose_watchdog.py | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)
 create mode 100644 Drone/visual_pose_watchdog.py

diff --git a/Drone/visual_pose_watchdog.py b/Drone/visual_pose_watchdog.py
new file mode 100644
index 0000000..94f06f1
--- /dev/null
+++ b/Drone/visual_pose_watchdog.py
@@ -0,0 +1,29 @@
+import rospy
+import time
+from std_srvs.srv import Trigger
+from geometry_msgs.msg import PoseStamped
+
+land = rospy.ServiceProxy('/land', Trigger)
+
+visual_pose_last_timestamp = 0
+visual_pose_timeout = 1.
+
+rospy.init_node('visual_pose_watchdog')
+rospy.loginfo('visual_pose_watchdog inited')
+
+def visual_pose_callback(data):
+    global visual_pose_last_timestamp
+    visual_pose_last_timestamp = data.header.stamp.to_sec()
+
+def watchdog_callback(event):
+    global visual_pose_last_timestamp
+    if (time.time() - visual_pose_last_timestamp) > visual_pose_timeout and visual_pose_last_timestamp != 0:
+        rospy.loginfo('Visual pose data is too old, landing...')
+        land()
+
+rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)
+
+rospy.Timer(rospy.Duration(0.5), watchdog_callback)
+
+rospy.spin()
+    
\ No newline at end of file