Drone control node update - takeoff, landing, comments

scripts/tello_control.py

@@ -12,17 +12,79 @@ class TelloVision(object):
 	def __init__(self):
 		# Start ROS node 
 		rospy.init_node('tello_vision_node')
-		self.is_processed = False
-
+		self.pub_takeoff = rospy.Publisher('/tello/takeoff', Empty,  queue_size=1, latch=True)
+		self.pub_land = rospy.Publisher('/tello/land', Empty,  queue_size=1, latch=False)
+		self.sub_status = rospy.Subscriber('/tello/status',TelloStatus,self.refresh_status_cb,queue_size=1)
+		self.status = TelloStatus()
+		self.bridge = CvBridge()
+		self.image_sub = rospy.Subscriber("/tello/camera/image_raw",Image,self.callback)
 
 	def spin(self):
+		# Function not let the node shutdown until an interrupt comes in (Keyboard interrupt or shutdown signal).
 		rospy.spin()
 
+	def refresh_status_cb(self,status_msg):
+		# This callback reads the status message from the drone and refreshes the local status variable.
+		self.status = status_msg
+	
+	def takeoff_cmd(self):
+		# This function sends the takeoff command to the drone (publishes an empty message to the takeoff topic).
+		self.pub_takeoff.publish()
+
+	def land_cmd(self):
+		# This function sends the land command to the drone (publishes an empty message to the land topic).
+		self.pub_land.publish()
+
+	def is_flying(self):
+		# Returns with the value from TelloStatus showing if the drone is flying.
+		return self.status.is_flying
+
+	def is_landing(self):
+		# Returns the value showing whether the landing is in progress.
+		# During the execution of the land command the fly_mode value is 12.
+		return self.status.fly_mode == 12
+
+	def is_taking_off(self):
+		# Returns the value showing whether the takeoff is in progress.
+		# During the execution of the takeoff command the fly_mode value is 11.
+		return self.status.fly_mode == 11
+
+	def is_idle(self):
+		# Returns the value showing whether the drone is in idle state.
+		# During the execution of the idle state the fly_mode value is 6.
+		# At the beginning until a status message comes in the value is 0.
+		return self.status.fly_mode in (0,6)
+
 if __name__ == '__main__':
 	try:
-		node = TelloVision()
-		while not node.is_processed:
-			node.spin()
+		# Create the tello object
+		tello = TelloVision()
+		# Set sleep rate
+		rate = rospy.Rate(10)
+		# Take off the drone
+		while tello.is_idle():
+			# Send a take off command
+			tello.takeoff_cmd()
+			# Use sleep to let the value of self.status refreshed on current thread.
+			rate.sleep()		
+		while tello.is_taking_off():
+			# Wait until the drone is up and idle - there are ways doing this much more effective - ref. wait-notify
+			# Use sleep to let the value of self.status refreshed on current thread.
+			rate.sleep()
+		# Land the drone
+		while tello.is_idle():
+			# Send a land command
+			tello.land_cmd()
+			# Use sleep to let the value of self.status refreshed on current thread.
+			rate.sleep()		
+		while tello.is_landing():
+			# Wait until the drone is on the ground idle - there are ways doing this much more effective - ref. wait-notify
+			# Without this part the message can be dropped
+			# Use sleep to let the value of self.status refreshed on current thread.
+			rate.sleep()
 	except rospy.ROSInterruptException:
 		pass
+	except KeyboardInterrupt:
+		pass
 	rospy.signal_shutdown('Great Flying!')
+	
\ No newline at end of file