def IER(point, kdt):
tree = kdt.tree
points = kdt.points
p = LatLon(point[0], point[1])
dist, ind = tree.query([point], k=1)
ix = ind[0][0]
p2 = points[ix]
distance = gmaps.distance_matrix(point, p2, mode="driving")
netDistance = distance["rows"][0]["elements"][0]["distance"]["value"]
bestDistance = netDistance
i = 1
dist, ind = tree.query([point], k=i + 1)
ix = ind[0][i]
pe = points[ix]
nnPoint = LatLon(pe[0], pe[1])
euclideanDistance = p.distance(nnPoint) * 1000
while (euclideanDistance < bestDistance):
p2 = (pe[0], pe[1])
distance = gmaps.distance_matrix(point, p2, mode="driving")
netDistance = distance["rows"][0]["elements"][0]["distance"]["value"]
if (netDistance < bestDistance):
bestDistance = netDistance
i = i + 1
dist, ind = tree.query([point], k=i + 1)
ix = ind[0][i]
pe = points[ix]
nnPoint = LatLon(pe[0], pe[1])
lastED = euclideanDistance
euclideanDistance = p.distance(nnPoint) * 1000
while (lastED == euclideanDistance):
i = i + 1
dist, ind = tree.query([point], k=i + 1)
ix = ind[0][i]
pe = points[ix]
nnPoint = LatLon(pe[0], pe[1])
lastED = euclideanDistance
euclideanDistance = p.distance(nnPoint) * 1000
return bestDistance
def IER( point, kdt ): tree = kdt.tree points = kdt.points p = LatLon(point[0], point[1]) dist, ind = tree.query([point], k=1) ix = ind[0][0] p2 = points[ix] distance = gmaps.distance_matrix(point, p2, mode="driving") netDistance = distance["rows"][0]["elements"][0]["distance"]["value"] bestDistance = netDistance i = 1 dist, ind = tree.query([point], k = i+1) ix = ind[0][i] pe = points[ix] nnPoint = LatLon(pe[0], pe[1]) euclideanDistance = p.distance(nnPoint)*1000 while (euclideanDistance < bestDistance): p2 = (pe[0], pe[1]) distance = gmaps.distance_matrix(point, p2, mode="driving") netDistance = distance["rows"][0]["elements"][0]["distance"]["value"] if (netDistance < bestDistance): bestDistance = netDistance i = i+1 dist, ind = tree.query([point], k = i+1) ix = ind[0][i] pe = points[ix] nnPoint = LatLon(pe[0], pe[1]) lastED = euclideanDistance euclideanDistance = p.distance(nnPoint)*1000 while (lastED == euclideanDistance): i = i+1 dist, ind = tree.query([point], k = i+1) ix = ind[0][i] pe = points[ix] nnPoint = LatLon(pe[0], pe[1]) lastED = euclideanDistance euclideanDistance = p.distance(nnPoint)*1000 return bestDistance
def set_equation_of_line(self, node_start, node_end):
# # setup your projections
# crs_wgs = proj.Proj(init='epsg:4326') # assuming you're using WGS84 geographic
# crs_bng = proj.Proj(init='epsg:26986') # use a locally appropriate projected CRS
#
# # then cast your geographic coordinate pair to the projected system
# x1, y1 = proj.transform(crs_wgs, crs_bng, node_start.longitude, node_start.latitude)
# x2, y2 = proj.transform(crs_wgs, crs_bng, node_end.longitude, node_end.latitude)
#39.947604
#-75.190101
standard_coord = LatLon(39.947604, -75.190101)
point1X = LatLon(39.947604, node_start.longitude)
point1Y = LatLon(node_start.latitude, -75.190101)
x1 = standard_coord.distance(point1X)
y1 = standard_coord.distance(point1Y)
point2X = LatLon(39.947604, node_end.longitude)
point2Y = LatLon(node_end.latitude, -75.190101)
x2 = standard_coord.distance(point2X)
y2 = standard_coord.distance(point2Y)
print("X Distance to node {} is {}".format(node_start.name, x1))
print("Y Distance to node {} is {}".format(node_start.name, y1))
print("X Distance to node {} is {}".format(node_end.name, x2))
print("Y Distance to node {} is {}".format(node_end.name, y2))
# if x2-x1 != 0:
# slope = (y2 - y1)/(x2 - x1)
# print("Slope is {}".format(slope))
points = [(x1, y1), (x2, y2)]
x_coords, y_coords = zip(*points)
A = vstack([x_coords, ones(len(x_coords))]).T
m, b = lstsq(A, y_coords)[0]
print("Node Start {} Node End {}".format(node_start.name,
node_end.name))
print("Line Solution is y = {m}x + {b}".format(m=m, b=b))
return m, b
def go_gps_target(self, lat=0, lon=0, altitude=0, x_velocity=0, y_velocity=0, z_velocity=0, x_aceleration=0, y_aceleration=0, z_aceleration=0, yaw=0, yaw_rate=0):
self.gps_target.pose.position.latitude = lat
self.gps_target.pose.position.longitude = lon
self.gps_target.pose.position.altitude = altitude
self.gps_target.pose.orientation.x = self.gps_target.pose.orientation.y = 0
self.gps_target.pose.orientation.z = 1
self.gps_target.pose.orientation.w = yaw
self.global_position_pub.publish(self.gps_target)
self.rate.sleep()
velocity = 1
# using the python function LatLon
inicial = LatLon(Latitude(self.global_pose.latitude), Longitude(self.global_pose.longitude))
final = LatLon(Latitude(lat), Longitude(lon))
inicial_distance = inicial.distance(final)
actual_dist = inicial_disatance
#initial_heading = palmyra.heading_initial(honolulu)
loop_rate = rospy.Rate(2)
K = 1/20.0
while abs(lat - self.global_pose.latitude) >= TOL_GLOBAL and abs(lon - self.global_pose.longitude) >= TOL_GLOBAL:
if actual_dist <= K*inicial_distance:
v = K - (K/actual_distance) # v vai de 0 ate K
self.gps_target.pose.position.latitude = self.global_pose.latitude + (v*(lat - self.global_pose.latitude))
inicial = LatLon(Latitude(self.global_pose.latitude), Longitude(self.global_pose.longitude))
actual_dist = inicial.distance(final)
else:
self.gps_target.pose.position.latitude = self.global_pose.latitude + ((K)*(lat - self.global_pose.latitude))
self.gps_target.pose.position.longitude = self.global_pose.longitude + ((K)*(lon - self.global_pose.longitude))
self.gps_target.pose.position.altitude = self.drone_pose.pose.position.z
self.global_position_pub.publish(self.gps_target)
self.loop_rate.sleep()
def get_closest_road(endpoint_node, curr_latitude, curr_longitude):
standard_coord = LatLon(39.947604, -75.190101)
currX = LatLon(39.947604, curr_longitude)
currY = LatLon(curr_latitude, -75.190101)
x_curr = standard_coord.distance(currX)
y_curr = standard_coord.distance(currY)
mindist = maxsize
min_road = None
for road in endpoint_node.neighborEdges:
distance_to_road = getDistanceToLine(road, x_curr, y_curr)
if distance_to_road < mindist:
mindist = distance_to_road
min_road = road
return min_road
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
