def run_demo(self):
# Initialize demonstration file
date_time = strftime("%H_%M_%m_%d_%Y", localtime())
file = open(date_time + ".csv", 'w')
columns = "{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n".format(
"tag_0_X", "tag_0_Y", "tag_1_X", "tag_1_Y", "tag_2_X", "tag_2_Y",
"tag_3_X", "tag_3_Y", "tag_4_X", "tag_4_Y", "tag_5_X", "tag_6_Y",
"speedX", "speedY", "speedZ", "roll", "pitch", "yaw", "altitude",
"human_roll", "human_pitch", "human_yaw", "human_gaz")
# write header
file.write("{}".format(columns))
# start drone
self.connect()
while not self.js.button_states['a']:
pass
self.takeoff()
# main control loop
self.continue_flying = True
while self.continue_flying:
# poll from the xbox controller
if self.js.button_states['b']:
self.continue_flying = False
# playground area for messing with certain drone stuff for now
# change drone camera angle
if self.js.axis_states['hat0y']:
cameraAction = self.drone(
gimbal.set_target(
gimbal_id=0,
control_mode="velocity",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="relative",
pitch=-1 * self.js.axis_states['hat0y'],
roll_frame_of_reference="none",
roll=0.0,
)).wait()
# take as step and receive obs
action = self.get_human_action()
obs = self.step(action, render=True)
# log file
obs_str = "{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n".format(
obs[0], obs[1], obs[2], obs[3], obs[4], obs[5], obs[6], obs[7],
obs[8], obs[9], obs[10], obs[11], obs[12], obs[13], obs[14],
obs[15], obs[16], obs[17], obs[18], action[0], action[1],
action[2], action[3])
file.write(obs_str)
# close out
self.land()
while not self.js.button_states['b']:
pass
file.close()
self.disconnect()
def move_gimbal(attitude):
drone(
gimbal.set_target(gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="absolute",
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=attitude,
roll_frame_of_reference="absolute",
roll=0.0)).wait()
def gimbal_target(drone, abs_ang):
drone(
set_target(gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="absolute",
yaw=0,
pitch_frame_of_reference="absolute",
pitch=abs_ang,
roll_frame_of_reference="absolute",
roll=0))
def set_gimbal(drone,set_mode,set_yaw,set_pitch,set_roll):
drone(gimbal.set_target(
gimbal_id=0,
control_mode=set_mode,
yaw_frame_of_reference="absolute",
yaw=set_yaw,
pitch_frame_of_reference="absolute",
pitch=set_pitch,
roll_frame_of_reference="absolute",
roll=set_roll,
)).wait()
def gimbal_target(drone, abs_ang):
# print(' ')
# print(colored(("Gimbal angle: ", abs_ang), "green"))
drone(
set_target(gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="absolute",
yaw=0,
pitch_frame_of_reference="absolute",
pitch=abs_ang,
roll_frame_of_reference="absolute",
roll=0))
def CommonSetStandardCamera(self):
if not self._on_test:
self._drone(gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=0,
roll_frame_of_reference="none",
roll=0.0,
)).wait()
self.camera_pitch = 0;
else :
self.my_log.info("Mode simulation: informations reçues")
def camera_down(self):
# move camera downward
print('moving camera')
cameraAction = self.drone(
gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="relative",
pitch=-90.0,
roll_frame_of_reference="none",
roll=0.0,
)).wait()
if not cameraAction.success():
raise RuntimeError("Cannot set gimbal velocity target")
def takeoff():
display_message('Taking off...')
assert drone(TakeOff()).wait().success()
display_message('Takeoff successful')
# Set gimbal to attitude so that it looks straight
drone(
gimbal.set_target(gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="absolute",
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=0,
roll_frame_of_reference="absolute",
roll=0.0)).wait()
global gimbal_attitude
gimbal_attitude = 0.0
takeoff_button.config(state="disabled")
enable_movement_buttons()
def __init__(self):
super().__init__()
with open("%s/../server_config.json" % (dir_path)) as json_file:
data = json.load(json_file)
self.images_path = data["images_path"]
self.suppressLogs()
self.drone = olympe.Drone(ANAFI_IP)
self.drone.connection()
self.timeout = 3
self.flying = False
self.drone(
set_target(gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="relative",
yaw=0,
pitch_frame_of_reference="relative",
pitch=-90,
roll_frame_of_reference="relative",
roll=0.0)).wait()
def ManualTiltCamera(self,params):
if isinstance(params, dict):
params = DroneCommandParams(**params)
if self._manual_unit :
if not self._on_test:
self._drone(gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=self.camera_pitch+params.pitch,
roll_frame_of_reference="none",
roll=0.0,
)).wait()
self.camera_pitch = self.camera_pitch +params.pitch
else :
self.my_log.info("Mode simulation: informations reçues {} ".format(params.to_string()))
else :
self.my_log.info("Mode automatique : commande {} ignorée ".format(params.to_string()))
def test_asyncaction():
with olympe.Drone(DRONE_IP) as drone:
drone.connect()
# Start a flying action asynchronously
flyingAction = drone(
TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5) >>
moveBy(10, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5) >> Landing())
# Start video recording while the drone is flying
if not drone(start_recording(cam_id=0)).wait().success():
assert False, "Cannot start video recording"
# Send a gimbal pitch velocity target while the drone is flying
cameraAction = drone(
gimbal.set_target(
gimbal_id=0,
control_mode="velocity",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="none",
pitch=0.1,
roll_frame_of_reference="none",
roll=0.0,
)).wait()
if not cameraAction.success():
assert False, "Cannot set gimbal velocity target"
# Wait for the end of the flying action
if not flyingAction.wait().success():
assert False, "Cannot complete the flying action"
# Stop video recording while the drone is flying
if not drone(stop_recording(cam_id=0)).wait().success():
assert False, "Cannot stop video recording"
# Leaving the with statement scope: implicit drone.disconnect() but that
# is still a good idea to perform the drone disconnection explicitly
drone.disconnect()
def start(self):
print('<< start() >>')
# Connect the the drone
self.drone.connection()
# 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)
self.drone(MaxAltitude(self.max_altitude)).wait()
self.drone(
gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="none", # None instead of absolute
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=self.gimbal_pitch,
roll_frame_of_reference="none", # None instead of absolute
roll=0.0)).wait()
def AutomaticSetCameraDown(self):
"""On descend la caméra"""
if not self._manual_unit :
if not self._on_test:
self._drone(gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=-1,
roll_frame_of_reference="none",
roll=0.0,
)).wait()
self.camera_pitch = -1;
else :
self.my_log.info("Mode simulation: informations reçues")
time.sleep(1)
self._brain_client.emit('on_command_success', {"name":self.my_name, "command":"AutomaticSetCameraDown"})
else :
self.my_log.info("Mode Manuel : Commande ignorée")
>> FlyingStateChanged(state="hovering", _timeout=5)
>> moveBy(3, 0, 0, 0)
# >> FlyingStateChanged(state="hovering", _timeout=5) # there is currently a bug in firmware. Just rely on moveBy expection for now
>> Landing()
)
# Start video recording while the drone is flying
if not drone(start_recording(cam_id=0)).wait().success():
raise RuntimeError("Cannot start video recording")
# Send a gimbal pitch velocity target while the drone is flying
cameraAction = drone(gimbal.set_target(
gimbal_id=0,
control_mode="velocity",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="none",
pitch=0.1,
roll_frame_of_reference="none",
roll=0.0,
)).wait()
if not cameraAction.success():
raise RuntimeError("Cannot set gimbal velocity target")
# Wait for the end of the flying action
if not flyingAction.wait().success():
print(drone.get_state(FlyingStateChanged)['state'])
raise RuntimeError("Cannot complete the flying action")
# Stop video recording while the drone is flying
if not drone(stop_recording(cam_id=0)).wait().success():
def take_Map_Photo_Move(drone, th_mov, th_gim, th_R0, xf, dxm, dzm):
photo_saved = drone(photo_progress(result="photo_saved", _policy="wait"))
# Pass - 1:
drone(moveBy(0, 0, dzm, 0)
>> FlyingStateChanged(state="hovering", _timeout=5)).wait() # Initial Height
m_AR = []
AMPU_ID = []
drone(gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="relative",
yaw=0.0,
pitch_frame_of_reference="relative",
pitch=th_gim,
roll_frame_of_reference="relative",
roll=0.0)
>> olympe.messages.gimbal.attitude(pitch_relative=th_gim, _timeout=5)
).wait()
D_th = th_mov
x_arr = arange(0, xf, dxm)
xlen = len(x_arr)
th_arr1 = linspace(0, D_th, xlen)
th_arr = th_arr1 + th_R0
dth = th_arr1[1]
# take a photo burst and get the associated media_id
for ix in range(0, xlen-1):
xc, yc, zc = R3_Mat_Cords(th_arr[ix], dxm)
drone(
take_photo(cam_id=0) & moveBy(xc, yc, zc, dth)
>> FlyingStateChanged(state="hovering", _timeout=5)).wait()
photo_saved.wait()
idp = photo_saved.received_events().last().args["media_id"]
m_AR.append(idp)
# Pass - 2:
drone(moveBy(0, 0, -2*dzm, 0)
>> FlyingStateChanged(state="hovering", _timeout=5)).wait() # Initial Height
drone(gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="relative",
yaw=0.0,
pitch_frame_of_reference="relative",
pitch=-2*th_gim,
roll_frame_of_reference="relative",
roll=0.0)
>> olympe.messages.gimbal.attitude(pitch_relative=-2*th_gim, _timeout=5)
).wait()
D_th2 = -th_mov
th_arr2 = linspace(0, D_th2, xlen)
dth2 = th_arr2[1]
# take a photo burst and get the associated media_id
for ix1 in range(0, xlen-1):
xc, yc, zc = R3_Mat_Cords(th_arr[xlen-ix1-1], -dxm)
drone(
take_photo(cam_id=0) & moveBy(xc, yc, zc, dth2)
>> FlyingStateChanged(state="hovering", _timeout=5)).wait()
photo_saved.wait()
idp = photo_saved.received_events().last().args["media_id"]
m_AR.append(idp)
#media_id = photo_saved.received_events().last().args["media_id"]
nph = len(m_AR)
## HEre
# download the photos associated with this media id
os.chdir("/home/rnallapu/code/Results")
#media_info_response = requests.get(ANAFI_MEDIA_API_URL + m_AR)
#media_info_response = requests.get(ANAFI_MEDIA_API_URL.join(m_AR))
for inph in range(0, nph-1):
AMPU_ID.append(ANAFI_MEDIA_API_URL + m_AR[inph])
media_info_response = requests.get(AMPU_ID[2])
media_info_response.raise_for_status()
print(AMPU_ID)
print('Action Completed')
download_dir = tempfile.mkdtemp()
Res_dir = filecreation()
#tempfile.gettempdir()
for resource in media_info_response.json()["resources"]:
image_response = requests.get(ANAFI_URL + resource["url"], stream=True)
download_path = os.path.join(download_dir, resource["resource_id"])
print('Hello')
print(download_path)
image_response.raise_for_status()
with open(download_path, "wb") as image_file:
shutil.copyfileobj(image_response.raw, image_file)
#shutil.copy2(download_path, "/home/rnallapu/code/Photos/")
shutil.copy2(download_path, Res_dir)
def customShadowCallback_Delta(payload, responseStatus, token):
# payload is a JSON string ready to be parsed using json.loads(...)
# in both Py2.x and Py3.x
print(responseStatus)
payloadDict = json.loads(payload)
print("++++++++DELTA++++++++++")
print("state: " + str(payloadDict["state"]))
print("version: " + str(payloadDict["version"]))
print("+++++++++++++++++++++++\n\n")
if 'gimbal_pitch' in payloadDict['state'].keys():
droneThing.drone(
gimbal.set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="none", # None instead of absolute
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=payloadDict['state']['gimbal_pitch'],
roll_frame_of_reference="none", # None instead of absolute
roll=0.0,
)).wait()
print('updated gimbal_pitch')
if 'gimbal_speed' in payloadDict['state'].keys():
droneThing.drone(
gimbal.set_max_speed(
gimbal_id=0,
yaw=0.0,
pitch=payloadDict['state']['gimbal_speed'],
roll=0.0,
)).wait()
print('updated gimbal_speed')
if 'max_vertical_speed' in payloadDict['state'].keys():
droneThing.drone(
MaxVerticalSpeed(
payloadDict['state']['max_vertical_speed'])).wait()
print('updated max_vertical_speed')
if 'max_rotation_speed' in payloadDict['state'].keys():
droneThing.drone(
MaxRotationSpeed(
payloadDict['state']['max_rotation_speed'])).wait()
print('updated max_rotation_speed')
if 'max_tilt' in payloadDict['state'].keys():
droneThing.drone(MaxTilt(payloadDict['state']['max_tilt'])).wait()
print('updated max_tilt')
if 'flight_altitude' in payloadDict['state'].keys():
droneThing.flight_altitude = payloadDict['state']['flight_altitude']
print('updated flight_altitude')
if 'detection_enabled' in payloadDict['state'].keys():
droneThing.detection_enabled = payloadDict['state'][
'detection_enabled']
print('updated detection_enabled')
if 'detection_mode' in payloadDict['state'].keys():
droneThing.detection_mode = payloadDict['state']['detection_mode']
print('updated detection_mode')
if 'targeted_car' in payloadDict['state'].keys():
droneThing.targeted_car = payloadDict['state']['targeted_car']
droneThing.initBB = None
droneThing.trackerInitialized = False
print('updated targeted_car')
print("*******************************************************************************************************")
print("GOING TO PICTURE POINT")
print("*******************************************************************************************************")
drone(
moveTo(latitude=pictureLatitude,longitude=pictureLongitude,altitude=pictureAltitude, orientation_mode=0, heading=0)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
#Rotate camera down
print("*******************************************************************************************************")
print("AIMING CAMERA")
print("*******************************************************************************************************")
drone(set_target(
gimbal_id=0,
control_mode="position",
yaw_frame_of_reference="absolute", # None instead of absolute
yaw=0.0,
pitch_frame_of_reference="absolute",
pitch=-90.0,
roll_frame_of_reference="absolute", # None instead of absolute
roll=0.0,
)).wait()
#Set camera mode
drone(
set_camera_mode(cam_id=0, value=1)
).wait()
#Set photo mode
drone(
set_photo_mode(cam_id=0, mode=0, format=0, file_format=0, burst=0, bracketing=0, capture_interval=0)
).wait()
# #Take picture
# drone(
# take_photo(cam_id=0)
