def calc_heading(position, waypoint):
pos = LatLon(Latitude(position.latitude), Longitude(position.longitude))
wp = LatLon(Latitude(waypoint[0]), Longitude(waypoint[1]))
# get angle between pos and wp
angle = pos.heading_initial(wp)
# check if these directions are good for sailing
if angle < 45:
angle = 45
elif angle > 135:
angle = 135
return angle
def calc_heading(position, waypoint):
pos = LatLon(Latitude(position.latitude), Longitude(position.longitude))
wp = LatLon(Latitude(waypoint[0]), Longitude(waypoint[1]))
# get angle between pos and wp
angle = pos.heading_initial(wp)
# check if these directions are good for sailing
if angle < 45:
angle = 45
elif angle > 135:
angle = 135
return angle
def update_direction(self):
if self.latitude == "" or self.longitude == "":
return # there's no position data
# rospy.loginfo("%s %s Current latlon: %s,%s" % (id(self), self.hexident, self.latitude, self.longitude))
# rospy.loginfo("%s Current direction: %s" % (self.hexident, str(self.last2positions)))
if len(self.last2positions) == 1:
if self.last2positions[0] == (self.latitude, self.longitude):
return
if len(self.last2positions) == 2:
if self.last2positions[1] == (self.latitude, self.longitude):
return
else:
self.last2positions.pop(0)
self.last2positions.append((self.latitude, self.longitude))
if len(self.last2positions) == 2: # determine direction!
p1 = LatLon(Latitude(self.last2positions[0][0]),
Longitude(self.last2positions[0][1]))
p2 = LatLon(Latitude(self.last2positions[1][0]),
Longitude(self.last2positions[1][1]))
self.heading = p1.heading_initial(p2)
if self.heading < 0:
self.heading += 360
if self.heading > 337.5 or self.heading < 22.5:
self.direction = "N"
elif 22.5 < self.heading < 67.5:
self.direction = "NE"
elif 67.5 < self.heading < 112.5:
self.direction = "E"
elif 112.5 < self.heading < 157.5:
self.direction = "SE"
elif 157.5 < self.heading < 202.5:
self.direction = "S"
elif 202.5 < self.heading < 247.5:
self.direction = "SW"
elif 247.5 < self.heading < 292.5:
self.direction = "W"
elif 292.5 < self.heading < 337.5:
self.direction = "NW"
# rospy.loginfo("Points: p1=%s p2=%s" % (p1,p2))
rospy.loginfo("Radar: New heading for hexident=%s: %f %s" %
(self.hexident, self.heading, self.direction))
def how_close(a_lat,a_lon, b_lat, b_lon):
a_latlon = LatLon(a_lat, a_lon)
b_latlon = LatLon(b_lat, b_lon)
dist = a_latlon.distance(b_latlon)
bearing = a_latlon.heading_initial(b_latlon)
return dist, bearing
