From 30b488010f7797d44fc397fed5fbf5d1b6b97dd3 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Thu, 7 Dec 2017 01:16:59 +0300
Subject: [PATCH] Post vpe 0:0:0 if no local position to initialize it

---
 clever/src/aruco_vpe.py | 30 ++++++++++++++++++++++++++++--
 1 file changed, 28 insertions(+), 2 deletions(-)

diff --git a/clever/src/aruco_vpe.py b/clever/src/aruco_vpe.py
index a5e589e1..e9d6ca26 100755
--- a/clever/src/aruco_vpe.py
+++ b/clever/src/aruco_vpe.py
@@ -40,10 +40,15 @@ def send_transform(transform, child_frame_id):
     tf_broadcaster.sendTransform(transform)
 
 
+vpe_posted = False
+
+
 def publish_vpe(pose):
-    global last_published
     stamp = pose.header.stamp
 
+    global last_published, vpe_posted
+    vpe_posted = True
+
     def lookup_transform(target_frame, source_frame):
         return tf_buffer.lookup_transform(target_frame, source_frame, stamp, LOOKUP_TIMEOUT)
 
@@ -76,5 +81,26 @@ def publish_vpe(pose):
 
 
 rospy.Subscriber('aruco_pose/pose', PoseStamped, publish_vpe, queue_size=1)
+
+
+local_pose = None
+
+
+def handle_pose(data):
+    global local_pose
+    local_pose = data
+
+
+rospy.Subscriber('mavros/local_position/pose', PoseStamped, handle_pose, queue_size=1)
+
+
 rospy.loginfo('aruco_vpe inited')
-rospy.spin()
+r = rospy.Rate(5)
+
+
+while not rospy.is_shutdown():
+    if not vpe_posted and not local_pose:
+        ps.header.stamp = rospy.get_rostime()
+        vision_position_pub.publish(ps)
+
+    r.sleep()