def compute_reference_traj(self):
# Compute trajectory at time = now
time = rospy.get_time()
# stay hover at the last waypoint position
if self.index == self.m:
hovering_point = self.waypoint[-1]
ref_trajectory = hv.hovering_traj(hovering_point)
# TODO: local planner hovering -> need to generate new global trajectory
# In other word, reference time should be time which is subtracted by last time
else:
ref_time = time - self.last_time
solution = self.solution[
self.n * (self.order + 1) * self.index: self.n * (self.order + 1) * (self.index + 1)]
ref_trajectory = df.get_trajectory(solution, self.order, self.time[self.index], ref_time)
# If the time is gone, segment should be changed
# Index increases.
if (time - self.last_time) > self.time[self.index]:
self.index = self.index + 1 # trajectory to next gate
self.last_time = time
return ref_trajectory
def compute_reference_traj(self):
# Compute trajectory at time = now
time = rospy.get_time()
# stay hover at the last waypoint position
if self.index == self.waypoint - 1:
pos = self.position[-1]
x = pos[0]
y = pos[1]
z = pos[2]
psi = pos[3]
pos = np.array([x, y, z])
vel = np.array([0, 0, 0])
acc = np.array([0, 0, 0])
jerk = np.array([0, 0, 0])
snap = np.array([0, 0, 0])
yaw = psi
yaw_dot = 0
yaw_ddot = 0
flat_output = [pos, vel, acc, jerk, snap, yaw, yaw_dot, yaw_ddot]
ref_trajectory = df.compute_ref(flat_output)
# In this polynomial, time variable has range [0, 1] at every segment
# In other word, reference time should be time which is subtracted by last time
else:
ref_time = time - self.last_time
solution = self.solution[self.n * (self.order + 1) *
self.index:self.n * (self.order + 1) *
(self.index + 1)]
ref_trajectory = df.get_trajectory(solution, self.order,
self.time[self.index], ref_time)
# Index increases.
if (time - self.last_time) > self.time[self.index]:
self.index = self.index + 1 # trajectory to next gate
self.last_time = time
return ref_trajectory