class Despega:
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.despega_sub = rospy.Subscriber('Despega', Empty, self.callback)
def callback(self, data):
self.controller.SendTakeoff()
class AutoDrive(object):
def __init__(self):
self.image = None
self.imageLock = Lock()
self.bridge = CvBridge()
rospy.init_node('ardrone_keyboard_controller')
self.controller = BasicDroneController()
self.image_sub = rospy.Subscriber("/ardrone/bottom/image_raw", Image,
self.reciveImage)
self.action = 0 # 0 takeoff status , 1 go foward
self.status = -1 # drone status , 0 flying,-1 landed
def reciveImage(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.processImage(cv_image)
except CvBridgeError as e:
print(e)
#process recived image ,change the drone's action
def processImage(self, img):
#cv2.imwrite('drone.jpg',img)
cv2.imshow("Image window", img)
cv2.waitKey(3)
self.action = 1
def drive(self):
while not rospy.is_shutdown():
# sleep 1 second to wait the drone initial
rate = rospy.Rate(1)
rate.sleep()
if self.status == -1:
print "send take off"
self.controller.SendTakeoff()
self.status = 0
elif self.action == 1:
print "send go foward"
self.controller.SetCommand(pitch=1)
else:
pass
self.controller.Land()
class TakeoffDirective(AbstractDroneDirective):
# sets up this directive
def __init__(self):
self.controller = BasicDroneController("Takeoff Directive")
# given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 1 if algorithm is finished and the drone is taking off
#
# A tuple of (xspeed, yspeed, yawspeed, zspeed):
# indicating the next instructions to fly the drone
#
# An image reflecting what is being done as part of the algorithm
def RetrieveNextInstruction(self, image, navdata):
self.controller.SendTakeoff()
rospy.logwarn("Drone is taking off")
return 1, (0, 0, 0, 0), image, (None, None), 0, 0, None
print "Down"
controller.SetCommand(0, 0, 0, -0.3)
rospy.sleep(3)
print "Stop"
controller.SetCommand(0, 0, 0, 0)
rospy.sleep(1)
if __name__ == '__main__':
rospy.init_node('preplanned_path', anonymous=True)
pub1 = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10)
controller = BasicDroneController()
controller.StartSendCommand()
rospy.sleep(1)
controller.SendTakeoff()
print "Take off"
controller.StartSendCommand()
rospy.sleep(5)
zigzag2()
print "Down"
controller.SetCommand(0, 0, 0, -0.25)
rospy.sleep(3)
#rospy.sleep(1)
print "Land"
controller.SendLand()
#pub2 = rospy.Subscriber("/visualization_marker", Marker, detect_tag)
class KeyboardController(object):
def __init__(self):
# initalize gui window (pygame)
pygame.init()
self.screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption("Keyboard Controller")
(self.screen).fill(GREY)
background = pygame.image.load(expanduser("~")+"/drone_ws/src/ardrone_lab/src/resources/KeyboardCommands4.png")
self.screen.blit(background,[0,0])
pygame.display.update()
self.keyPub = rospy.Publisher('/controller/keyboard',ROSString)
# setup controller + its variables
self.controller = BasicDroneController("Keyboard")
self.speed = 1
self.pitch = 0
self.roll = 0
self.yaw_velocity = 0
self.z_velocity = 0
def startController(self):
# while gui is still running, continusly polls for keypresses
gameExit = False
while not gameExit:
for event in pygame.event.get():
# checking when keys are pressing down
if event.type == pygame.KEYDOWN and self.controller is not None:
self.keyPub.publish(str(event.key))
if event.key == pygame.K_t:
#switches camera to bottom when it launches
self.controller.SendTakeoff()
self.controller.SwitchCamera(1)
print( "Takeoff")
elif event.key == pygame.K_g:
self.controller.SendLand()
rospy.logwarn("-------- LANDING DRONE --------")
print ("Land")
elif event.key == pygame.K_ESCAPE:
self.controller.SendEmergency()
print ("Emergency Land")
elif event.key == pygame.K_c:
self.controller.ToggleCamera()
print ("toggle camera")
elif event.key == pygame.K_z:
self.controller.FlatTrim()
else:
if event.key == pygame.K_w:
self.pitch = self.speed
elif event.key == pygame.K_s:
self.pitch = self.speed*-1
elif event.key == pygame.K_a:
self.roll = self.speed
print ("Roll Left")
elif event.key == pygame.K_d:
self.roll = self.speed*-1
print ("Roll Right")
elif event.key == pygame.K_q:
self.yaw_velocity = self.speed
print ("Yaw Left")
elif event.key == pygame.K_e:
self.yaw_velocity = self.speed*-1
print ("Yaw Right")
elif event.key == pygame.K_r:
self.z_velocity = self.speed*1
print ("Increase Altitude")
elif event.key == pygame.K_f:
self.z_velocity = self.speed*-1
print ("Decrease Altitude")
self.controller.SetCommand(self.roll, self.pitch,
self.yaw_velocity, self.z_velocity)
if event.type == pygame.KEYUP:
if (event.key == pygame.K_w or event.key == pygame.K_s or event.key == pygame.K_a or event.key == pygame.K_d
or event.key == pygame.K_q or event.key == pygame.K_e or event.key == pygame.K_r or event.key == pygame.K_f):
self.pitch = 0
self.roll = 0
self.z_velocity = 0
self.yaw_velocity = 0
self.controller.SetCommand(self.roll, self.pitch,
self.yaw_velocity, self.z_velocity)
if event.type == pygame.QUIT:
gameExit = True
pygame.display.quit()
pygame.quit()
class AutoDrive(object):
def __init__(self):
self.rotZ = 0
self.Zlock = Lock()
self.image = None
self.imageLock = Lock()
self.bridge = CvBridge()
rospy.init_node('ardrone_keyboard_controller')
self.controller = BasicDroneController()
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.receiveNavdata)
self.image_sub = rospy.Subscriber("/ardrone/bottom/image_raw", Image,
self.reciveImage)
self.action = 0 # 0 takeoff status , 1 go foward
def reciveImage(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.processImage(cv_image)
except CvBridgeError as e:
print(e)
#process recived image ,change the drone's action
def processImage(self, img):
cv2.imwrite('./drone.jpg', img)
self.action = 1
def turnLeft(self):
#self.Zlock.acquire()
self.controller.SetCommand(pitch=0)
while (self.rotZ <= 179):
self.controller.SetCommand(yaw_velocity=1)
self.controller.SetCommand(yaw_velocity=0)
#self.Zlock.release()
def receiveNavdata(self, navdata):
self.Zlock.acquire()
self.rotZ = navdata.rotZ
print self.rotZ
self.Zlock.release()
def drive(self):
start_time = rospy.get_rostime()
while not rospy.is_shutdown():
curent = rospy.get_rostime()
if (curent - start_time) < rospy.Duration(1, 0):
self.controller.SendTakeoff()
self.status = 0
elif (curent - start_time) < rospy.Duration(22, 0):
self.controller.SetCommand(pitch=1)
self.status = 1
elif (curent - start_time) < rospy.Duration(23, 0):
self.controller.SetCommand(pitch=0)
self.status = 2
elif (curent - start_time) < rospy.Duration(24, 0):
#self.controller.SetCommand(yaw_velocity=2)
self.turnLeft()
self.status = 3
rate = rospy.Rate(1)
elif (curent - start_time) < rospy.Duration(40.5, 0):
self.controller.SetCommand(pitch=1)
self.status = 4
else:
self.controller.SetCommand(pitch=0)
self.controller.SendLand()
class Fly:
def __init__(self):
#Drone's odometry subscriber
self.sub_odom = rospy.Subscriber("/drone_control/odom_converted",
Odometry, self.callback_odom)
#Aruco's data subscribers
self.sub_aruco = rospy.Subscriber("/drone_control/aruco_detected",
Bool, self.callback_detected)
self.sub_aruco_center = rospy.Subscriber("/drone_control/aruco_center",
Float32MultiArray,
self.callback_center)
self.sub_aruco_id = rospy.Subscriber("/drone_control/aruco_id", Int8,
self.callback_id)
#Drone's linear and anglar velocity publishers
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.pub_angle = rospy.Publisher('/drone_control/drone_angle',
Float32,
queue_size=1)
self.controller = BDC()
self.twist = Twist()
#Drone's coordinates
self.drone_x = 0
self.drone_y = 0
self.drone_z = 0
#angle towards aruco marker
self.angle = 0
#Difference between drone's angle and its destination
self.diff_angle = 0
self.aruco_center = (320, 180)
self.Image_center = (320, 180)
self.old_aruco_center = None
#The variable is incremented each frame the aruco marker is detected
self.detection_counter = 0
#Aruco markers' coordinates dictionary
self.dest_dict = OrderedDict()
self.dest_dict['1'] = (3, -14)
self.dest_dict['10'] = (-14, 2)
self.dest_dict['20'] = (-4, 13)
self.dest_dict['50'] = (11, 4)
self.dest_dict['100'] = (0, 0)
self.dest_id = self.dest_dict.keys()[0]
self.marker_coords = self.dest_dict[self.dest_id]
#Additional boolean variables
self.aruco_id = None
self.detected = False
self.rotated = False
self.on_target = False
def callback_id(self, message):
#Get aruco's id
self.aruco_id = message.data
def callback_center(self, message):
#Get aruco's center coordinates (in pixels)
self.aruco_center = message.data
def rotate_2_angle_and_fly(self):
#Rotate towards the next marker
if self.diff_angle >= 5:
self.controller.SetCommand(yaw_velocity=0.3, pitch=0)
elif self.diff_angle <= -5:
self.controller.SetCommand(yaw_velocity=-0.3, pitch=0)
#If the angle is correct fly forward to the next marker
else:
self.controller.SetCommand(pitch=1)
def land_on_target(self):
#Calculate the difference (in pixels) between image's center and detected aruco's center
x_diff = self.Image_center[0] - self.aruco_center[0]
y_diff = self.Image_center[1] - self.aruco_center[1]
x_diff = x_diff / (self.Image_center[0])
y_diff = y_diff / (self.Image_center[1])
#Drone's landing movement is slower if aruco marker's visibility keeps getting obstructed
if self.detection_counter <= 2:
self.controller.SetCommand(pitch=x_diff * 0.3,
roll=y_diff * 0.3,
z_velocity=-0.5)
else:
self.controller.SetCommand(pitch=x_diff,
roll=y_diff,
z_velocity=-0.3)
#If the drone is low enough commence a built-in landing sequence
if self.drone_z < 1.5:
self.controller.SetCommand(pitch=0, roll=0, z_velocity=0)
self.controller.SendLand()
#If the drone has landed, prepare for the next destination
if self.dest_dict and self.controller.status == DroneStatus.Landed:
self.on_target = True
self.dest_dict.popitem(last=False)
self.dest_id = self.dest_dict.keys()[0]
self.marker_coords = self.dest_dict[self.dest_id]
self.detection_counter = 0
rospy.sleep(3)
self.on_target = False
else:
print("Finished flying!")
def callback_odom(self, odom):
#Get drone's coordinates
self.drone_x = odom.pose.pose.position.x
self.drone_y = odom.pose.pose.position.y
self.drone_z = odom.pose.pose.position.z
#Find an angle towards aruco marker
angle = math.degrees(
self.calculate_angle((self.drone_x, self.drone_y),
self.marker_coords))
#Get an angular difference between destination's angle and drone's current angle
self.diff_angle = angle - self.controller.rotation
print("kat:", self.controller.rotation)
print("kurs:", angle)
print("diff:", self.diff_angle)
#Take off if the drone is on the ground
if self.controller.status == DroneStatus.Landed and self.dest_dict and not self.on_target:
self.controller.SendTakeoff()
#Keep going up if the drone is flying low
if self.controller.status == DroneStatus.Flying and self.drone_z < 10:
self.controller.SetCommand(z_velocity=2)
#If the drone is high enough, fly towards the next destination
elif self.controller.status == DroneStatus.Flying and self.drone_z >= 10:
self.rotate_2_angle_and_fly()
def callback_detected(self, message):
self.detected = message.data
#If the proper aruco marker is detected, begin landing on it
if self.detected and self.aruco_id == int(self.dest_id):
self.detection_counter += 1
self.land_on_target()
def calculate_angle(self, point1, point2):
#Calculate an angle towards aruco marker
angle = math.atan2((point2[1] - point1[1]), (point2[0] - point1[0]))
print('wspolrzedne drona:', point1[0], point1[1])
print('wspolrzedne markera:', point2[0], point2[1])
return angle
