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()
