def start(self):
# Connect the the drone
self.drone.connect()
self.drone(setPilotingSource(source="Controller")).wait()
#self.drone.zoom_control_mode(0,1) #level,velocity
#self.drone.white_balance_temperature(0)
# self.drone(olympe.enums.camera.set_zoom_target(0,1))
# You can record the video stream from the drone if you plan to do some
# post processing.
self.drone.set_streaming_output_files(
h264_data_file=os.path.join(self.tempd, 'h264_data.264'),
h264_meta_file=os.path.join(self.tempd, 'h264_metadata.json'),
# Here, we don't record the (huge) raw YUV video stream
# raw_data_file=os.path.join(self.tempd,'raw_data.bin'),
# raw_meta_file=os.path.join(self.tempd,'raw_metadata.json'),
)
# Setup your callback functions to do some live video processing
self.drone.set_streaming_callbacks(
raw_cb=self.yuv_frame_cb,
h264_cb=self.h264_frame_cb,
start_cb=self.start_cb,
end_cb=self.end_cb,
flush_raw_cb=self.flush_cb,
)
# Start video streaming
self.drone.start_video_streaming()
def switch_manual(self):
msg_rpyt = SkyControllerCommand()
msg_rpyt.header.stamp = rospy.Time.now()
msg_rpyt.header.frame_id = '/body'
self.pub_skycontroller.publish(msg_rpyt)
# button: 0 = RTL, 1 = takeoff/land, 2 = back left, 3 = back right
self.drone(
mapper.grab(buttons=(0 << 0 | 0 << 1 | 0 << 2 | 1 << 3),
axes=0)).wait() # bitfields
self.drone(setPilotingSource(source="SkyController")).wait()
rospy.loginfo("Control: Manual")
def __init__(self):
# Create the olympe.Drone object from its IP address
self.drone = olympe.Drone(CONTROLLER_IP, drone_type=od.ARSDK_DEVICE_TYPE_ANAFI4K)
self.drone(setPilotingSource(source="Controller")).wait()
self.tempd = tempfile.mkdtemp(prefix="olympe_streaming_test_")
print("Olympe streaming output dir: {}".format(self.tempd))
self.h264_frame_stats = []
self.h264_stats_file = open(os.path.join(self.tempd, 'h264_stats.csv'), 'w+')
self.h264_stats_writer = csv.DictWriter(self.h264_stats_file, ['fps', 'bitrate'])
self.h264_stats_writer.writeheader()
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
# initialize the known distance from the camera to the object (referring to picture)
self.KNOWN_DISTANCE = 25.1
# initialize the known object width, which in this case, the barcode(referring to QR code in picture)
self.KNOWN_WIDTH = 2.16535
# change path to your own path
self.image = cv2.imread("/home/dragonfly/Downloads/Using Controller/images/barcode.jpg")
# decode the QR code in the picture
self.foundBarcode = pyzbar.decode(self.image)
# loop the found barcode and get the width (in pixels) for the QR code in the picture, then calculate the focal length
for QRCode in self.foundBarcode:
(x, y, w, h) = QRCode.rect
self.focalLength = (w * self.KNOWN_DISTANCE) / self.KNOWN_WIDTH
self.request_post = Anafi_Request_Post()
self.scanning_decode = Anafi_Scanning()
# comment/uncomment this part if want to read location from txt file
# self.listOfLocation = self.request_post.readLocation()
# comment/uncomment this part if want to get location from the server
self.listOfLocation =self.request_post.getLocation()
# print the list of location to show the initialize location in the warehouse
print(self.listOfLocation)
# flag for the current location and status
self.currentLocation = None
self.currentLocationStatus = False
# initializing the barcode data list for storing the list of barcode that will be scanned later
self.barcodeDataList = []
super().__init__()
super().start()
def switch_offboard(self):
# button: 0 = RTL, 1 = takeoff/land, 2 = back left, 3 = back right
# axis: 0 = yaw, 1 = trottle, 2 = roll, 3 = pithch, 4 = camera, 5 = zoom
if self.drone.get_state(pilotingSource)[
"source"] == PilotingSource_Source.SkyController:
self.drone(
mapper.grab(buttons=(1 << 0 | 0 << 1 | 1 << 2 | 1 << 3),
axes=(1 << 0 | 1 << 1 | 1 << 2 | 1 << 3 | 0 << 4
| 0 << 5))) # bitfields
self.drone(setPilotingSource(source="Controller")).wait()
rospy.loginfo("Control: Offboard")
else:
self.switch_manual()
# connect to simulated drone in SPHINX.
drone = olympe.Drone("10.202.0.1") # create Drone instance
else:
# connect to physical drone through the Sky Controller
# you can also connect to drone using Wifi using ip address 192.168.42.1
drone = olympe.Drone("192.168.53.1", mpp=True, drone_type=od.ARSDK_DEVICE_TYPE_ANAFI4K)
# connect to the drone and check if connection was established
response = drone.connection()
if not response.OK: # if connection failed, exit
print("Connection was not successful")
exit()
# set piloting source to controller, only when connecting through the Sky Controller
if not SIMULATION:
drone(setPilotingSource(source="Controller")).wait()
print("TakingOff")
response = drone(TakeOff()).wait() # send takeoff command and wait for response or timeout
if not response.success(): # if takeoff command was not successful, exit
print("Takeoff was not successful")
exit()
time.sleep(5) # wait until drone is hovering (we will see how to use the SDK to do this more efficiently later)
print("Landing")
response = drone(Landing()).wait() # send landing command and wait for response or timeout
if not response.success(): # if landing command was not successful, exit
print("Landing was not successful")
exit()
import olympe
from olympe.messages.skyctrl.CoPiloting import setPilotingSource
from olympe.messages.skyctrl.CoPilotingState import pilotingSource
from olympe.messages.ardrone3.PilotingState import PositionChanged
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged
from olympe.messages.ardrone3.GPSSettingsState import HomeChanged
import time
SKYCTRL_IP = "192.168.53.1"
if __name__ == "__main__":
drone = olympe.Drone(SKYCTRL_IP)
drone.connect()
drone(setPilotingSource(0)).wait()
#print("Piloting Source : ", drone.get_state(pilotingSource))
# Wait for GPS fix
drone(GPSFixStateChanged(_policy='wait'))
print("GPS position before take-off :", drone.get_state(HomeChanged))
o_alt = ""
o_lat = "0.0"
o_lon = "0.0"
while True:
#drone(PCMD(1, 1, 1, 1, 2, 0))
print(" --- Current GPS Position --- ")
alt = drone.get_state(PositionChanged)['altitude']
print("Altitude : ", alt)
lat = drone.get_state(PositionChanged)['latitude']
print("Latitude : ", lat)
