def pp_firmware_to_ros(pp):
def piece_to_ros(piece):
poly = QuadcopterTrajectoryPoly()
poly.duration = rospy.Duration.from_sec(piece.duration)
for i in range(8):
poly.poly_x.append(firm.poly4d_get(piece, 0, i))
poly.poly_y.append(firm.poly4d_get(piece, 1, i))
poly.poly_z.append(firm.poly4d_get(piece, 2, i))
poly.poly_yaw.append(firm.poly4d_get(piece, 3, i))
return poly
return [piece_to_ros(firm.pp_get_piece(pp, i)) for i in range(pp.n_pieces)]
def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
traj = firm.piecewise_traj()
traj.t_begin = 0
traj.timescale = 1.0
traj.shift = firm.mkvec(0, 0, 0)
traj.n_pieces = len(trajectory.polynomials)
traj.pieces = firm.malloc_poly4d(len(trajectory.polynomials))
for i, poly in enumerate(trajectory.polynomials):
piece = firm.pp_get_piece(traj, i)
piece.duration = poly.duration
for coef in range(0, 8):
firm.poly4d_set(piece, 0, coef, poly.px.p[coef])
firm.poly4d_set(piece, 1, coef, poly.py.p[coef])
firm.poly4d_set(piece, 2, coef, poly.pz.p[coef])
firm.poly4d_set(piece, 3, coef, poly.pyaw.p[coef])
self.trajectories[trajectoryId] = traj
