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