def upload_trajectory(scf: SyncCrazyflie, data: Dict): cf = scf.cf # type: Crazyflie print('Starting upload') trajectory_mem = scf.cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] TRAJECTORY_MAX_LENGTH = 31 trajectory = data['trajectory'] if len(trajectory) > TRAJECTORY_MAX_LENGTH: raise ValueError("Trajectory too long for drone {:s}".format( cf.link_uri)) for row in trajectory: duration = row[0] x = Poly4D.Poly(row[1:9]) y = Poly4D.Poly(row[9:17]) z = Poly4D.Poly(row[17:25]) yaw = Poly4D.Poly(row[25:33]) trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw)) print('Calling upload method') uploader = Uploader(trajectory_mem) uploader.upload() print('Defining trajectory.') cf.high_level_commander.define_trajectory(trajectory_id=1, offset=0, n_pieces=len( trajectory_mem.poly4Ds))
def upload_trajectory(cf, trajectory_id, trajectory): trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] total_duration = 0 for row in trajectory: duration = row[0] x = Poly4D.Poly(row[1:9]) y = Poly4D.Poly(row[9:17]) z = Poly4D.Poly(row[17:25]) yaw = Poly4D.Poly(row[25:33]) trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw)) total_duration += duration Uploader().upload(trajectory_mem) cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.trajectory)) return total_duration
def upload_trajectory(cf, trajectory_id, trajectory): trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] total_duration = 0 for row in trajectory: duration = row[0] x = Poly4D.Poly(row[1:9]) y = Poly4D.Poly(row[9:17]) z = Poly4D.Poly(row[17:25]) yaw = Poly4D.Poly(row[25:33]) trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw)) total_duration += duration upload_result = Uploader().upload(trajectory_mem) if not upload_result: print('Upload failed, aborting!') sys.exit(1) cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.poly4Ds)) return total_duration