def plan_cb(self, goal):
# Wait for map to be created
while not rospy.is_shutdown():
if self.map is not None:
break
rospy.loginfo_throttle(5, 'Planning: Waiting for map...')
self.wait_rate.sleep()
rospy.loginfo('Planning: map recieved!')
# Create waypoints for the planner to pass through
waypoints = self.create_waypoints(self.map.gates)
start_wp = waypoints.pop(0)
# Run path planning
planner = RRT(self.map)
traj = planner.plan_traj(start_wp, waypoints)
while traj is None and not rospy.is_shutdown():
rospy.logwarn('RRT: Restarting RRT...')
traj = planner.plan_traj(start_wp, waypoints)
# Convert trajectory of Beziers to dd241909_msgs/Trajectory
traj_pieces = []
for bezier in traj:
piece = TrajectoryPolynomialPiece()
for c in bezier.coeffs:
piece.poly_x.append(c.x)
piece.poly_y.append(c.y)
piece.poly_z.append(c.z)
piece.poly_yaw.append(0.0)
piece.duration = rospy.Duration(bezier.T)
traj_pieces.append(piece)
traj_msg = Trajectory()
traj_msg.header.frame_id = 'map'
traj_msg.header.stamp = rospy.Time.now()
traj_msg.pieces = traj_pieces
# Set yaw values of traj_msg inplace
self.set_yaw_gates(traj_msg, start_wp, traj)
# self.set_yaw_rotating(traj_msg, start_wp, waypoints[-1])
# Publish trajectory
self.traj_pub.publish(traj_msg)
# Visualise trajectory in rviz
if self.use_rviz:
publish_traj_to_rviz(traj_msg)
self.plan_server.set_succeeded(SimpleResult(message=''))
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):
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):
"""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)