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