def initialize_wps(self):
# start
self.add_wp(0.00, 0.0, 0.60, deg_to_rad(0))
# gate 1
self.add_wp(0.12, 2.88 - 0.30, 0.60, deg_to_rad(15))
self.add_wp(0.12, 2.88 - 0.00, 0.60, deg_to_rad(30))
self.add_wp(0.12, 2.88 + 0.30, 0.60, deg_to_rad(45))
# U-turn after gate 1
self.add_wp(-0.15 - 0.10, 2.88 + 0.50, 0.60, deg_to_rad(65))
self.add_wp(-0.40 - 0.10, 2.88 + 0.60, 0.60, deg_to_rad(90))
self.add_wp(-0.70 - 0.10, 2.88 + 0.30, 0.60, deg_to_rad(120))
self.add_wp(-1.00, 2.88 - 0.30, 0.60, deg_to_rad(150))
self.add_wp(-1.35, 2.00, 0.60, deg_to_rad(180))
# gate 2
self.add_wp(-1.35, 0.70 + 0.30, 0.40, deg_to_rad(180))
self.add_wp(-1.35, 0.70 + 0.00, 0.40, deg_to_rad(205))
self.add_wp(-1.35, 0.70 - 0.30, 0.40, deg_to_rad(220))
# U-turn after gate 2
self.add_wp(-0.60, 0.70 - 0.30, 0.60, deg_to_rad(260))
self.add_wp(-0.30, 0.70 - 0.60, 0.60, deg_to_rad(300))
self.add_wp(0.00, 0.0, 0.60, deg_to_rad(340))
self.wps_path = path_generator(self.wps, 0.08)
self.wps = self.wps_path.poses
def initialize_wps(self):
# NWU coordinate
# start
self.add_wp(0.50, -0.15, 1.15, deg_to_rad(0))
# right side of the wall
self.add_wp(1.24, -0.70, 1.15, deg_to_rad(0))
self.add_wp(3.10, -0.53, 1.05, deg_to_rad(0))
self.add_wp(4.00, 0.80, 1.05, deg_to_rad(0))
# poles
self.add_wp(5.00, 1.10, 1.05, deg_to_rad(0))
self.add_wp(6.60, 1.62, 0.85, deg_to_rad(0))
# trees
self.add_wp(9.00, 1.50, 0.85, deg_to_rad(0))
self.add_wp(11.20, 2.60, 1.00, deg_to_rad(0))
# before tunnel
self.add_wp(12.50, 2.80, 1.15, deg_to_rad(-5))
self.add_wp(13.00, 3.00, 1.15, deg_to_rad(-30))
self.add_wp(14.00, 2.80, 1.15, deg_to_rad(-75))
self.add_wp(14.65, 1.80, 1.15, deg_to_rad(-110))
self.wps_path = path_generator(self.wps, 0.05)
self.wps = self.wps_path.poses
rospy.loginfo(len(self.wps))
rospy.loginfo("Finish interpolating the waypoints")
def mission(self):
'''
Set the waypoint for Mission ONE, from start to in front of the tunnel
# NWU coordinate
'''
self.idx_wp = 0
# start
# self.add_wp( -1.60, 0.80, 0.40, deg_to_rad(0))
# front of first boxes
self.add_wp(-0.80, 0.60, 0.80, deg_to_rad(0))
self.add_wp(-0.24, -1.00, 1.20, deg_to_rad(0))
#front left of second boxes
self.add_wp(1.05, -0.10, 1.00, deg_to_rad(0))
self.add_wp(2.12, 0.95, 0.80, deg_to_rad(0))
# end of the way
self.add_wp(3.28, 0.60, 0.80, deg_to_rad(0))
# behind second boxes
self.add_wp(3.00, -0.80, 1.50, deg_to_rad(0))
self.add_wp(2.40, -0.80, 2.50, deg_to_rad(0))
self.add_wp(0.88, 0.70, 1.50, deg_to_rad(0))
# end of the mission
self.add_wp(-1.18, -0.46, 1.00, deg_to_rad(0))
self.wps_path = path_generator(self.wps, 0.05)
self.wps = self.wps_path.poses
rospy.loginfo("M1: Finish interpolating the waypoints")
def mission_one_wps(self):
'''
Set the waypoint for Mission ONE, from start to in front of the tunnel
# NWU coordinate
'''
self.idx_wp = 0
# start
self.add_wp(0.50, -0.15, 1.15, deg_to_rad(0))
# right side of the wall
self.add_wp(1.24, -0.70, 1.15, deg_to_rad(0))
self.add_wp(3.10, -0.53, 1.05, deg_to_rad(0))
self.add_wp(4.00, 0.80, 1.05, deg_to_rad(0))
# poles
self.add_wp(5.00, 1.10, 1.05, deg_to_rad(0))
self.add_wp(6.60, 1.62, 0.85, deg_to_rad(0))
# trees
self.add_wp(9.00, 1.50, 0.85, deg_to_rad(0))
self.add_wp(11.20, 2.60, 1.00, deg_to_rad(0))
# before tunnel
self.add_wp(12.50, 2.80, 1.15, deg_to_rad(-5))
self.add_wp(13.00, 3.00, 1.15, deg_to_rad(-30))
self.add_wp(14.00, 2.80, 1.15, deg_to_rad(-75))
self.add_wp(14.65, 2.00, 1.15, deg_to_rad(-120))
self.wps_path = path_generator(self.wps, 0.05)
self.wps = self.wps_path.poses
rospy.loginfo("M1: Finish interpolating the waypoints")
def initialize_wps(self):
# NWU coordinate
# start
self.add_wp(0.50, -0.15, 1.15, deg_to_rad(0))
# right side of the wall
self.add_wp(1.24, -0.70, 1.15, deg_to_rad(0))
self.add_wp(3.10, -0.53, 1.05, deg_to_rad(0))
self.add_wp(4.00, 0.80, 1.05, deg_to_rad(0))
# poles
self.add_wp(5.00, 1.10, 1.05, deg_to_rad(0))
self.add_wp(6.60, 1.62, 0.85, deg_to_rad(0))
# trees
self.add_wp(9.00, 1.50, 0.85, deg_to_rad(0))
self.add_wp(11.20, 2.60, 1.00, deg_to_rad(0))
# before tunnel
self.add_wp(12.50, 2.80, 1.15, deg_to_rad(-5))
self.add_wp(13.00, 3.00, 1.15, deg_to_rad(-30))
self.add_wp(14.00, 2.80, 1.15, deg_to_rad(-75))
self.add_wp(14.65, 2.00, 1.15, deg_to_rad(-120))
# in tunnel
# self.add_wp(14.00, 0.80, 1.15, deg_to_rad(-90))
# self.add_wp(13.80, 0.00, 1.15, deg_to_rad(-90))
# self.add_wp(13.70, -0.80, 1.15, deg_to_rad(-90))
# self.add_wp(13.60, -1.80, 1.15, deg_to_rad(-90))
# self.add_wp(13.70, -0.80, 1.15, deg_to_rad(-60))
# self.add_wp(13.60, -1.80, 1.15, deg_to_rad(-20))
# self.add_wp(14.00, 0.80, 1.15, deg_to_rad(-115))
# self.add_wp(13.80, 0.00, 1.15, deg_to_rad(-130))
# self.add_wp(13.70, -0.80, 1.15, deg_to_rad(-155))
# self.add_wp(13.60, -1.80, 1.15, deg_to_rad(-180)) # should finish turning and be able to localize itself again
# self.add_wp(13.00, -2.00, 1.15, deg_to_rad(-180))
# windows
# self.add_wp(11.50, -1.90, 1.15, deg_to_rad(-180))
# self.add_wp(10.80, -1.35, 1.15, deg_to_rad(-180))
# self.add_wp( 9.50, -0.80, 1.15, deg_to_rad(-180))
# self.add_wp( 7.50, -0.80, 1.15, deg_to_rad(-180))
self.wps_path = path_generator(self.wps, 0.05)
self.wps = self.wps_path.poses
rospy.loginfo(len(self.wps))
rospy.loginfo("Finish interpolating the waypoints")
def mission_three_wps(self):
'''
Set the waypoint for Mission Three, from outside of the tunnel to the window
# NWU coordinate
'''
self.idx_wp = 0
self.wps = []
# windows
self.add_wp(10.80, -1.35, 1.15, deg_to_rad(-180))
self.add_wp(10.80, -0.70, 1.15, deg_to_rad(-180))
self.wps_path = path_generator(self.wps, 0.05)
self.wps = self.wps_path.poses
rospy.loginfo("M3: Finish interpolating the waypoints")
def initialize_wps(self):
# start
self.add_wp(-1.70, -0.60, 0.51, deg_to_rad(-90))
# gate 1
self.add_wp(1.33 - 0.30, -0.7, 0.60, deg_to_rad(-90))
self.add_wp(1.33 + 0.60, -0.7, 0.60, deg_to_rad(-90))
# U-turn after gate1
self.add_wp(2.12, 0.00, 0.51, deg_to_rad(0))
self.add_wp(2.00, 0.80, 0.51, deg_to_rad(90))
# self.add_wp(1.60, 0.80, 0.51, deg_to_rad(90))
# gate 2
self.add_wp(-0.80 + 0.30, 0.76, 0.52, deg_to_rad(90))
self.add_wp(-0.80 - 0.60, 0.76, 0.52, deg_to_rad(90))
# U-turn after gate2
self.add_wp(-2.30, 0.06, 0.52, deg_to_rad(130))
self.add_wp(-2.00, -0.60, 0.52, deg_to_rad(180))
self.wps = path_generator(self.wps, 0.08).poses