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
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()
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()
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
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()
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()