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
Asked by Anonymous on 2019-09-20 16:10:04 UTC
Answers
You created a publisher for the /cmd_vel
topic, but you're not using it. You need to publish a speed at which you want to move, in the direction that you want to move using a geometry_msgs::Twist
message.
So, in this case you need to set the linear.z
to a value greater than 0
and up to1
. Once you reach your desired altitude, then publish a geometry_msgs::Twist
message with every value (linear.{x,y,z}
and angular.{x,y,z}
) set to 0
which will enable the auto-hover mode and let your drone hover in (hopefully) one spot. (See the docs for more information about sending commands to your drone).
Again, the Command_directions
method needs to publish these commands otherwise you'll see the behavior that you've described. For example,
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.command.publish(self.Control) # Add this line
self.rate.sleep()
Asked by jayess on 2019-09-20 23:21:40 UTC
Comments
@Anonymous if this answered your question, then please click on the check mark next to the answer
Asked by jayess on 2019-09-24 19:35:32 UTC
Comments