develop track lost msg when drone can't localize

uzumal · Oct 17, 2022 · a6a3c51 · a6a3c51
1 parent 7a9014b
commit a6a3c51
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 10 deletions.
diff --git a/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/cloud_map_saver.py b/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/cloud_map_saver.py
@@ -4,7 +4,8 @@
 
 import math
 from sensor_msgs.msg import PointCloud2
-from std_msgs.msg import Empty 
+from std_msgs.msg import Empty, String
+from tf2_msgs.msg import TFMessage
 from geometry_msgs.msg import PoseStamped, Point
 from sensor_msgs.msg import Image
 import struct
@@ -47,8 +48,7 @@ def __init__(self, root):
 
         self.bridge = CvBridge()
 
-        self.uzu_pub = rospy.Publisher('uzu_data', PointCloud2, queue_size=10)
-
+        self.track_data_pub = rospy.Publisher('track_data', String, queue_size=1)
 
         self.file_path = rospy.get_param('~OUT_FILE_PATH')
         self.cloud_topic_name = rospy.get_param('~CLOUD_TOPIC_NAME')
@@ -58,7 +58,6 @@ def __init__(self, root):
 #        self.cloud_server_topic_name = rospy.get_param('~CLOUD_SERVER_TOPIC_NAME')
 
         self.received_image = False
-
         # self.updateImageGui = threading.Thread(target = self._getGUIImage)
 
         self.root.wm_title("TELLO Viewer"+str(self.id))
@@ -149,6 +148,7 @@ def __init__(self, root):
         rospy.Subscriber(self.trigger_topic_name, Empty, self.save_cloud_callback)
         rospy.Subscriber(self.pose_topic_name, PoseStamped, self.pose_callback)
         rospy.Subscriber(self.camera_topic_name, Image, self.img_callback)
+        rospy.Subscriber('/tf', TFMessage, self.track_callback)
 
         # rospy.Subscriber(self.cloud_server_topic_name, PointCloud2, self.point_cloud_server1_callback)
 
@@ -417,8 +417,7 @@ def float_from_4_bytes(self, bytes_list):
 
     def point_cloud_callback(self, point_cloud):
         fields_str = str(point_cloud.fields)
-	print('-------------------------POINT-CLOUD-------------------------------------------')
-	print(fields_str)
+
         # file.write(fields_str+'\n')
 
         # point_cloud.data is list of uint8[].
@@ -460,6 +459,13 @@ def point_cloud_callback(self, point_cloud):
 			[file.write(','.join(element)+'\n') for element in self.list_of_pure_lines]
 
 
+    def track_callback(self, track_data):
+	if track_data.transforms[0].transform.translation.x == 0:
+	    if track_data.transforms[0].transform.translation.y == 0:
+	        self.track_data_pub.publish("track losts")
+	else:
+	    self.track_data_pub.publish("track exists")
+
     def point_cloud_server1_callback(self, point_cloud):
         fields_str = str(point_cloud.fields)
         # file.write(fields_str+'\n')

diff --git a/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/flock_driver.py b/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/flock_driver.py
@@ -134,8 +134,6 @@ def __init__(self):
         self.packet_data = ""
         self._drone.subscribe(self._drone.EVENT_VIDEO_FRAME, self.videoFrameHandler)
         rospy.loginfo(FlightData())
-	print('CvBridge: '+str(self._cv_bridge))
-	print('EVENTFRAME: '+str(self._drone.EVENT_VIDEO_FRAME))
         # rospy.on_shutdown(self.cleanup)
 
 

diff --git a/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/tello_keyboard.py b/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/tello_keyboard.py
@@ -280,9 +280,7 @@ def limit_twist(twist):
                         pub_land.publish()
                         continue
                     elif keyname == 'tab':
-			print('haitterunoka----')
                         pub_takeoff.publish()
-			print('uwaaaaaaaaaaa----')
                         continue
                     else:
                         if not keyname in list_of_pressed_keys: