How do I get drone to respond to different commands other than takeoff and land?
I am trying to write a code that allows the drone to first takeoff then increase its altitude before moving forward for a certian distance before landing again. The problem is that the drone takeoff and hovers and then lands without executing anyother commands written to it in the middle. I am running the test code in gazebo. What would be the best solution for this?
#!/usr/bin/env python
#import library ros
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from std_msgs.msg import String
from ardrone_autonomy.msg import Navdata
class AutoFlight():
def __init__(self):
self.status = ""
self.pub_takeoff = rospy.Publisher('/ardrone/takeoff',Empty,queue_size =10) #Drone Takeoff
self.land_drone = rospy.Publisher('ardrone/land',Empty,queue_size=10) #land drone
self.command = rospy.Publisher('/cmd_vel',Twist,queue_size = 10)
self.Reset = rospy.Publisher('/ardrone/reset',Empty,queue_size =10)
self.rate=rospy.Rate(10)
self.sleep_mode= rospy.sleep(10)
#self.Shutdown_mode = rospy.on_shutdown(self.land_drone)
def Take_off(self):
self.pub_takeoff.publish(Empty())
self.sleep_mode
def Land (self):
self.land_drone.publish(Empty())
self.sleep_mode
def Reset_drone(self):
self.Reset(Empty())
def Command_directions(self, lin_x=0,lin_y=0,lin_z=0,ang_x=0,ang_y=0,ang_z=0):
self.Control = Twist()
self.Control.linear.x = lin_x
self.Control.linear.y = lin_y
self.Control.linear.z = lin_z
self.Control.angular.x = ang_x
self.Control.angular.y = ang_y
self.Control.angular.z = ang_z
self.rate.sleep()
if __name__== "__main__":
rospy.init_node('my_drone_node',anonymous=True) #Node Initiaton
drone = AutoFlight()
forward_dist = 0 #Initialization of forward movement acceleration
altitude = 0 # Iniatialization of the altitude -
try:
drone.Take_off()
rospy.sleep(1)
drone.Command_directions(0,0,0,0,0,0)
while not rospy.is_shutdown():
if altitude < 40:
drone.Command_directions(0,0,1,0,0,0)
#altitude =+ 1
altitude = altitude + 1
rospy.sleep(1)
#print("altitude:" ,altitude)
drone.Command_directions(1,0,0,0,0,0)
elif altitude < 50:
drone.Command_directions(0,0,0,0,0,0)
print("we are hovering")
# if forward_dist < 15:
# drone.Command_directions (1,0,0,0,0,0)
# forward_dist +=1
altitude = altitude + 1
#drone.Land()
except rospy.ROSInterruptException:
pass