Ejemplo n.º 1
0
def interpret_mission_file(mission_list):
	boat_goal_points = []
	for i in range(len(mission_list)):
		mission = mission_list[i]			# The format for a mission should be ['type of mission', 'information passing parameter', 'goal_latitude', 'goal_longitude']
		if mission[0] == 1:					# 'type of mission' = 1 means do waypoint following
			goal_UTM = latlon_tools.lat_lon_to_UTM([float(mission[2]), float(mission[3])])
			goal_x_y = goal_UTM[0]
			boat_goal_points.append(goal_x_y)

		elif mission[0] == 2 and i == 0:	# reject the 'round buoy' mission if it is the first mission, because there is original directino to interpret around
			print 'Rejected' + str(mission) + "as a mission because you can't round the buoy as the first mission - you need to give the boat a starting waypoint that it can get point itself at the buoy from"
		elif mission[0] == 2:
			radius_of_round_buoy = 2		# this will create waypoints with a radius of 2 meters around the buoy

			buoy_UTM = latlon_tools.lat_lon_to_UTM([float(mission[2]), float(mission[3])])
			buoy_x_y = buoy_UTM[0]

			prev_wp_UTM = latlon_tools.lat_lon_to_UTM([float(mission_list[i-1][2]), float(mission_list[i-1][3])])
			prev_wp_x_y = prev_wp_UTM[0]

			angle_to_buoy = atan2(buoy_x_y[1] - prev_wp_x_y[1], buoy_x_y[0] - prev_wp_x_y[0])
			if mission[1] == 1:				# round the buoy clockwise
				offset1 = pi/2
				offset2 = 0
				offset3 = -pi/2
			else:							# round the buoy counterclockwise
				offset1 = -pi/2
				offset2 = 0
				offset3 = pi/2

			goal1_x_y = [buoy_x_y[0] + (radius_of_round_buoy * cos(angle_to_buoy + offset1)), buoy_x_y[1] + radius_of_round_buoy * sin(angle_to_buoy + offset1)]
			goal2_x_y = [buoy_x_y[0] + (radius_of_round_buoy * cos(angle_to_buoy + offset2)), buoy_x_y[1] + radius_of_round_buoy * sin(angle_to_buoy + offset2)]
			goal3_x_y = [buoy_x_y[0] + (radius_of_round_buoy * cos(angle_to_buoy + offset3)), buoy_x_y[1] + radius_of_round_buoy * sin(angle_to_buoy + offset3)]

			boat_goal_points.append(goal1_x_y)
			boat_goal_points.append(goal2_x_y)
			boat_goal_points.append(goal3_x_y)

		# elif mission[0] == 3:		# New people can define their own mission interpretations here
			# goal_x_y = [mission[2] + your stuff, mission[3] + your stuff]
			# boat_goal_points.append(goal_x_y)

		else:
			print str(mission) + " has an unknown type of mission"

	return boat_goal_points
Ejemplo n.º 2
0
	def __set_current_y(self, data):
#		rospy.loginfo("sensors.py: GPS sent %i latitude" % (data.data))
		self.current_y = latlon_tools.lat_lon_to_UTM([data.data, 0])[0][0]
		self.y_updated = 1
		self.__check_that_both_x_and_y_have_updated()
Ejemplo n.º 3
0
	def __set_current_x(self, data):
#		rospy.loginfo("sensors.py: GPS sent %i longitude" % (data.data))
		self.current_x = latlon_tools.lat_lon_to_UTM([0, data.data])[0][1]
		self.x_updated = 1
		self.__check_that_both_x_and_y_have_updated()
Ejemplo n.º 4
0
def interpret_mission_file(mission_list):
    boat_goal_points = []
    for i in range(len(mission_list)):
        mission = mission_list[
            i]  # The format for a mission should be ['type of mission', 'information passing parameter', 'goal_latitude', 'goal_longitude']
        if mission[
                0] == 1:  # 'type of mission' = 1 means do waypoint following
            goal_UTM = latlon_tools.lat_lon_to_UTM(
                [float(mission[2]), float(mission[3])])
            goal_x_y = goal_UTM[0]
            boat_goal_points.append(goal_x_y)

        elif mission[
                0] == 2 and i == 0:  # reject the 'round buoy' mission if it is the first mission, because there is original directino to interpret around
            print 'Rejected' + str(
                mission
            ) + "as a mission because you can't round the buoy as the first mission - you need to give the boat a starting waypoint that it can get point itself at the buoy from"
        elif mission[0] == 2:
            radius_of_round_buoy = 2  # this will create waypoints with a radius of 2 meters around the buoy

            buoy_UTM = latlon_tools.lat_lon_to_UTM(
                [float(mission[2]), float(mission[3])])
            buoy_x_y = buoy_UTM[0]

            prev_wp_UTM = latlon_tools.lat_lon_to_UTM(
                [float(mission_list[i - 1][2]),
                 float(mission_list[i - 1][3])])
            prev_wp_x_y = prev_wp_UTM[0]

            angle_to_buoy = atan2(buoy_x_y[1] - prev_wp_x_y[1],
                                  buoy_x_y[0] - prev_wp_x_y[0])
            if mission[1] == 1:  # round the buoy clockwise
                offset1 = pi / 2
                offset2 = 0
                offset3 = -pi / 2
            else:  # round the buoy counterclockwise
                offset1 = -pi / 2
                offset2 = 0
                offset3 = pi / 2

            goal1_x_y = [
                buoy_x_y[0] +
                (radius_of_round_buoy * cos(angle_to_buoy + offset1)),
                buoy_x_y[1] +
                radius_of_round_buoy * sin(angle_to_buoy + offset1)
            ]
            goal2_x_y = [
                buoy_x_y[0] +
                (radius_of_round_buoy * cos(angle_to_buoy + offset2)),
                buoy_x_y[1] +
                radius_of_round_buoy * sin(angle_to_buoy + offset2)
            ]
            goal3_x_y = [
                buoy_x_y[0] +
                (radius_of_round_buoy * cos(angle_to_buoy + offset3)),
                buoy_x_y[1] +
                radius_of_round_buoy * sin(angle_to_buoy + offset3)
            ]

            boat_goal_points.append(goal1_x_y)
            boat_goal_points.append(goal2_x_y)
            boat_goal_points.append(goal3_x_y)

        # elif mission[0] == 3:		# New people can define their own mission interpretations here
        # goal_x_y = [mission[2] + your stuff, mission[3] + your stuff]
        # boat_goal_points.append(goal_x_y)

        else:
            print str(mission) + " has an unknown type of mission"

    return boat_goal_points
Ejemplo n.º 5
0
 def __set_current_y(self, data):
     #		rospy.loginfo("sensors.py: GPS sent %i latitude" % (data.data))
     self.current_y = latlon_tools.lat_lon_to_UTM([data.data, 0])[0][0]
     self.y_updated = 1
     self.__check_that_both_x_and_y_have_updated()
Ejemplo n.º 6
0
 def __set_current_x(self, data):
     #		rospy.loginfo("sensors.py: GPS sent %i longitude" % (data.data))
     self.current_x = latlon_tools.lat_lon_to_UTM([0, data.data])[0][1]
     self.x_updated = 1
     self.__check_that_both_x_and_y_have_updated()