def execute_cb2(self, goal): speak(engine, "Spiral trajectory.") traj1 = uav_trajectory.Trajectory() traj1.loadcsv("helicoidale.csv") # helper variables #r = rospy.Rate(10) rospy.wait_for_message('/tf', tf2_msgs.msg.TFMessage, timeout=None) # append the seeds for the fibonacci sequence self._feedback.time_elapsed = Duration(5) self.success == False # publish info to the console for the user #rospy.loginfo('%s: Now with tolerance %i with current pose [%s]' % (self._action_name, goal.order, ','.join(map(str,self._feedback.sequence)))) # check that preempt has not been requested by the client # start executing the action # x = TurtleBot() TRIALS = 1 TIMESCALE = 0.5 for cf in self.allcfs.crazyflies: cf.takeoff(targetHeight=0.6, duration=3.0) cf.uploadTrajectory(0, 0, traj1) #timeHelper.sleep(2.5) self.allcfs.crazyflies[0].startTrajectory(0, timescale=TIMESCALE) #timeHelper.sleep(1.0) print("press button to continue...") self.swarm.input.waitUntilButtonPressed() self.allcfs.crazyflies[0].land(targetHeight=0.06, duration=2.0) #timeHelper.sleep(3.0) if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False break #now we test if he has reached the desired point. #self.takeoff_transition() if self.success == False: print("Not yet...") try: ###self._feedback.sequence.append(currentPose) # publish the feedback self._as.publish_feedback(self._feedback) #rospy.loginfo('%s: Now with tolerance %i with current pose [%s]' % (self._action_name, goal.order, ','.join(map(str,self._feedback.sequence)))) except rospy.ROSInterruptException: print("except clause opened") rospy.loginfo('Feedback did not go through.') pass if self.success == True: for cf in self.allcfs.crazyflies: #print(cf.id) #print("press button to continue") #self.swarm.input.waitUntilButtonPressed() cf.land(0.04, 2.5) print("Reached the perimeter!!") self._result.time_elapsed = Duration(5) self._result.updates_n = 1 rospy.loginfo('My feedback: %s' % self._feedback) rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result)
# cf.goTo([0.5, 0.5, height], 0, duration = 1.0) # time.sleep(5.0) # cf.goTo([0, 0.5, height], 0, duration = 1.0) # time.sleep(5.0) # cf.goTo([0, 0, height], 0, duration = 1.0) # time.sleep(5.0) #cf.land(targetHeight = 0.0, duration = 2.0) # time.sleep(2.0) # cf.land(targetHeight = 0.0, duration = 2.0) traj1 = uav_trajectory.Trajectory() traj1.loadcsv("takeoff.csv") traj2 = uav_trajectory.Trajectory() traj2.loadcsv("figure8.csv") cf.uploadTrajectory(0, 0, traj1) cf.startTrajectory(0, timescale=1.0) time.sleep(traj1.duration * 2.0) t_upload_start = time.time() cf.uploadTrajectory(1, len(traj1.polynomials), traj2) print("upload time taken: {}s".format(time.time() - t_upload_start)) cf.startTrajectory(1, timescale=2.0) time.sleep(traj2.duration * 1.0)
def generate_trajectory(data, num_pieces): piece_length = data[-1, 0] / num_pieces x0 = np.zeros(num_pieces * 8) constraints = [] # piecewise values and derivatives have to match for i in range(1, num_pieces): for order in range(0, 4): constraints.append({ 'type': 'eq', 'fun': func_eq_constraint_der, 'args': (i, piece_length, order) }) # zero derivative at the beginning and end for order in range(1, 3): constraints.append({ 'type': 'eq', 'fun': func_eq_constraint_der_value, 'args': (0, 0, 0, order) }) constraints.append({ 'type': 'eq', 'fun': func_eq_constraint_der_value, 'args': (num_pieces - 1, piece_length, 0, order) }) resX = scipy.optimize.minimize(func, x0, (data[:, 0], data[:, 1], piece_length), method="SLSQP", options={"maxiter": 1000}, constraints=constraints) resY = scipy.optimize.minimize(func, x0, (data[:, 0], data[:, 2], piece_length), method="SLSQP", options={"maxiter": 1000}, constraints=constraints) resZ = scipy.optimize.minimize(func, x0, (data[:, 0], data[:, 3], piece_length), method="SLSQP", options={"maxiter": 1000}, constraints=constraints) resYaw = scipy.optimize.minimize(func, x0, (data[:, 0], data[:, 4], piece_length), method="SLSQP", options={"maxiter": 1000}, constraints=constraints) traj = uav_trajectory.Trajectory() traj.polynomials = [ uav_trajectory.Polynomial4D( piece_length, np.array(resX.x[i * 8:(i + 1) * 8][::-1]), np.array(resY.x[i * 8:(i + 1) * 8][::-1]), np.array(resZ.x[i * 8:(i + 1) * 8][::-1]), np.array(resYaw.x[i * 8:(i + 1) * 8][::-1])) for i in range(0, num_pieces) ] traj.duration = data[-1, 0] return traj
if __name__ == "__main__": swarm = Crazyswarm() timeHelper = swarm.timeHelper allcfs = swarm.allcfs ids = [42] #[15, 16, 17, 18, 19, 20] #[15, 16, 17] #range(1, 6+1) trajIds = [1] #1, 2, 3, 4, 5, 6] #[1, 2, 3, 4, 5, 6] cfs = [allcfs.crazyfliesById[i] for i in ids] root = 'swap6v_pps' fnames = ['{0}/pp{1}.csv'.format(root, i) for i in trajIds] # trajs = [piecewise.loadcsv(fname) for fname in fnames] T = 0 for cf, fname in zip(cfs, fnames): traj = uav_trajectory.Trajectory() traj.loadcsv(fname) cf.uploadTrajectory(0, 0, traj) T = max(T, traj.duration) print("T: ", T * TIMESCALE) allcfs.takeoff(targetHeight=1.0, duration=2.0) timeHelper.sleep(2.5) for cf, trajId in zip(cfs, trajIds): pos = POSITIONS[trajId - 1] print(pos) cf.goTo(np.array(pos) + np.array(OFFSET), 0, 3.0) timeHelper.sleep(3.5) allcfs.startTrajectory(0, timescale=TIMESCALE)
self.stop() self.in_mission = False self.flight_state = States.MANUAL def start(self): """ Start the log """ #self.start_log("Logs", "NavLog.txt") #self.connect() print("starting connection") # self.connection.start() super().start() # Only required if they do threaded # while self.in_mission: # pass """ Stop the log """ #self.stop_log() if __name__ == "__main__": traj0 = uav_trajectory.Trajectory() traj0.loadcsv("figure8.csv") #conn = CrazyflieConnection('radio://0/80/2M') drone = ArenaFlyer() time.sleep(2) drone.start()