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): """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)