def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
pieces = []
for poly in trajectory.polynomials:
piece = TrajectoryPolynomialPiece()
piece.duration = rospy.Duration.from_sec(poly.duration)
piece.poly_x = poly.px.p
piece.poly_y = poly.py.p
piece.poly_z = poly.pz.p
piece.poly_yaw = poly.pyaw.p
pieces.append(piece)
self.uploadTrajectoryService(trajectoryId, pieceOffset, pieces)
def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
pieces = []
for poly in trajectory.polynomials:
piece = TrajectoryPolynomialPiece()
piece.duration = rospy.Duration.from_sec(poly.duration)
piece.poly_x = poly.px.p
piece.poly_y = poly.py.p
piece.poly_z = poly.pz.p
piece.poly_yaw = poly.pyaw.p
pieces.append(piece)
self.uploadTrajectoryService(trajectoryId, pieceOffset, pieces)
def upload_trajectory(self, trajectory):
pieces = []
for poly in trajectory.polynomials:
piece = TrajectoryPolynomialPiece()
piece.duration = rospy.Duration.from_sec(poly.duration)
piece.poly_x = poly.px.p
piece.poly_y = poly.py.p
piece.poly_z = poly.pz.p
piece.poly_yaw = poly.pyaw.p
pieces.append(piece)
self.__trajectory_id = self.__trajectory_id + 1
self.__uploadTrajectoryService(self.__trajectory_id,
self.__piece_offset, pieces)
self.__piece_offset += len(trajectory.polynomials)
def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
"""Uploads a piecewise polynomial trajectory for later execution.
See uav_trajectory.py for more information about piecewise polynomial
trajectories.
Args:
trajectoryId (int): ID number of this trajectory. Multiple
trajectories can be uploaded. TODO: what is the maximum ID?
pieceOffset (int): TODO(whoenig): explain this.
trajectory (:obj:`pycrazyswarm.uav_trajectory.Trajectory`): Trajectory object.
"""
pieces = []
for poly in trajectory.polynomials:
piece = TrajectoryPolynomialPiece()
piece.duration = rospy.Duration.from_sec(poly.duration)
piece.poly_x = poly.px.p
piece.poly_y = poly.py.p
piece.poly_z = poly.pz.p
piece.poly_yaw = poly.pyaw.p
pieces.append(piece)
self.uploadTrajectoryService(trajectoryId, pieceOffset, pieces)