def test_sample_test(self):
"""Sample test method.
Test methods should be named 'test*'.
"""
self.assertAlmostEqual(calc.distance(self.p1, self.p2),
1111949.2664455848)
def think(self):
""" Finds a route to the next waypoint.
The route is optimal given constant wind conditions.
Keyword arguments:
self -- The caller, the instance
Side effects:
- Sets instance variables
"""
a_s = ship_data.boat_heading
lon_s = ship_data.boat_lon
lat_s = ship_data.boat_lat
lon_e = self.current_target[1]
lat_e = self.current_target[0]
a_e = 0
wind_heading = ship_data.wind_heading
def T(x):
""" Gets the time estimate to the waypoint.
Wraps the total_time function.
Keyword arguments:
x -- A latitude, longitude position
Returns:
A time estimate of time to the next waypoint
"""
lat = x[0]
lon = x[1]
return(time_estimator.total_time(lat_s, lon_s, a_s, lat, lon, lat_e, lon_e, wind_heading))
# Random unlabelled constants ftw
dTotal = calc.distance([lat_s, lon_s], [lat_e, lon_e]) / (111 * 1000.0)
# Get the best midpoint
self.current_fastest_midpoint = minimizer.minimize(
T,
[(lat_e + lat_s)/2,
(lon_e + lon_s)/2],
[dTotal/2, dTotal/2],
1,
lastMinX=self.current_fastest_midpoint)
#if it's better just to go through the midpoint, do it
if(T([(lat_s + lat_e)/2, (lon_s + lon_e)/2]) < T(self.current_fastest_midpoint)):
self.current_fastest_midpoint = [(lat_s + lat_e)/2, (lon_s + lon_e)/2]
self.current_desired_heading = calc.direction_to_point([ship_data.boat_lat, ship_data.boat_lon], self.current_fastest_midpoint)
ship_data.current_fastest_midpoint = self.current_fastest_midpoint
ship_data.current_desired_heading = self.current_desired_heading
ship_data.current_fastest_time_to_target = T(self.current_fastest_midpoint)
def within_radius_of_target(): """ Checks to see if waypoint is hit. This and check_waypoint_proximity should be merged, and should be with other navigation code. Returns: True if the ship is within the specified radius of the target, False otherwise """ current_point = [ship_data.boat_lat, ship_data.boat_lon] target_point = ship_data.target_points[ship_data.targetI] current_proximity = (calc.distance(current_point, target_point)) return calc.within_radius_of_target(current_point, target_point)
def dist(y1, x1, y2, x2):
""" Calculates distance using distance formula.
Keyword Arguments:
x1-- An x-coordinate
y1 -- A y-coordinate
x2 -- Another x-coordinate
y2 -- Another y-coordinate
Returns:
This function returns the distance between (x1, y1) and (x2, y2) as a double.
"""
# print(x1, y1, x2, y2)
return (calc.distance([y1, x1], [y2, x2]))
def dis():
r = request.get_json()
if (type(r) is not dict):
return '', 400
if (not 'first_pos' in r or not 'second_pos' in r):
return '', 400
if type(r['first_pos']) is str and not re.match(r"^robot#([1-9][0-9]*)$",
r['first_pos']):
return '', 400
if type(r['second_pos']) is str and not re.match(r"^robot#([1-9][0-9]*)$",
r['second_pos']):
return '', 400
# robot
if type(r['first_pos']) is str and re.match(r"^robot#([1-9][0-9]*)$",
r['first_pos']):
robot_id = int(r['first_pos'].split('#')[1])
# print(robot_id)
if (not robot_id in robot_positions):
return '', 424
r['first_pos'] = robot_positions[robot_id]
if type(r['second_pos']) is str and re.match(r"^robot#([1-9][0-9]*)$",
r['second_pos']):
robot_id = int(r['second_pos'].split('#')[1])
# print(robot_id)
if (not robot_id in robot_positions):
return '', 424
r['second_pos'] = robot_positions[robot_id]
#legacy
try:
r['first_pos'] = legacy(r['first_pos'])
r['second_pos'] = legacy(r['second_pos'])
except:
return '', 400
metric = "euclidean"
if ('metric' in r):
metric = r['metric']
if metric != 'euclidean' and metric != "manhattan":
return '', 400
# print(r['first_pos'])
# print(r['second_pos'])
return jsonify(
distance=distance(r['first_pos'], r['second_pos'], metric)), 200
def near():
r = request.get_json()
if (type(r) is not dict):
return '', 400
if not 'ref_position' in r:
return '', 400
k = 1
if ('k' in r):
if (r['k'] < 1):
return '', 400
k = r['k']
minimum = []
dist = {}
metric = "euclidean"
for id in robot_positions:
d = distance(robot_positions[id], r['ref_position'], metric)
minimum.append(id)
dist[id] = d
minimum.sort(key=lambda v: dist[v])
if len(robot_positions) == 0:
return jsonify(robot_ids=[]), 200
return jsonify(
robot_id=sorted(sorted(minimum[:k]), key=lambda v: dist[v])), 200
wangle = float(data.direction_true)
# print("\t\t\tWIND ANGLE IN: " + str(wangle) + " WIND ANGLE: " + str(ship_data.wind_heading))
wind_speed = float(data.wind_speed_meters)
# print("\t\t\tWIND SPEED IN: " + str(wind_speed) + " WIND SPEED: " + str(ship_data.wind_speed))
if("Latitude" in contents and data.latitude != None):
if(float(data.latitude) > 10):
ship_data.boat_lat = float(data.latitude)
# print("\t\t\t\t\tLATITUDE: " + str(data.latitude))
if("Longitude" in contents and data.longitude != None):
if(float(data.longitude) < -10):
ship_data.boat_lon = float(data.longitude)
ship_data.direction_to_target = calc.direction_to_point((ship_data.boat_lat, ship_data.boat_lon), ship_data.target_points[ship_data.targetI])
ship_data.current_distance_to_target = calc.distance((ship_data.boat_lat, ship_data.boat_lon), ship_data.target_points[ship_data.targetI])
# print("\t\t\t\t\tLONGITUDE: " + str(data.longitude))
# if("altitude" in contents):
# ship_data.boat_altitude = float(data.altitude)
if("heading" in contents and data.heading != None):
ship_data.boat_heading = (float(data.heading) + airmar_heading_offset)%360
# print("\t\t\t\t\tHEADING: " + str(data.heading))
if("speed" in contents and data.speed != None):
ship_data.boat_speed = float(data.speed)
except Exception, e:
print(e)
track_logger.add_line(str(e) + " caused by", "errors")