def find_start_route(self, dist): route_start = virt_cl() route_start.centerline.append(self.vehicle_point_in_lanelet_cl) for i in range(len(self.lanelet.centerline) - 1): if distance(self.lanelet.centerline[i], self.vehicle_point_in_lanelet_cl) < distance( self.lanelet.centerline[i], self.lanelet.centerline[i + 1]): for j in range(i + 1, len(self.lanelet.centerline)): route_start.centerline.append(self.lanelet.centerline[j]) if length_point_list(route_start.centerline) > dist: l1 = dist - length_point_list( route_start.centerline[:-1]) l2 = distance(route_start.centerline[-1], route_start.centerline[-2]) x1 = route_start.centerline[-2].x + ( l1 / l2) * (route_start.centerline[-1].x - route_start.centerline[-2].x) y1 = route_start.centerline[-2].y + ( l1 / l2) * (route_start.centerline[-1].y - route_start.centerline[-2].y) route_start.centerline[-1] = BasicPoint3d(x1, y1) break break return route_start
def __init__(self): print "loooooooooooooooooooooooooooooooo", rospy.get_param( "~vehicle_no") self.pub = rospy.Publisher(str("/gps_transform_" + str(rospy.get_param("~vehicle_no"))), NavSatFix, queue_size=10) self.pub_text = rospy.Publisher( str("/information_" + str(rospy.get_param("~vehicle_no"))), Marker, queue_size=10) self.sub_gps = rospy.Subscriber( str("/gps_data_" + str(rospy.get_param("~vehicle_no"))), NavSatFix, self.callback_gps) self.sub_imu = rospy.Subscriber( str("/imu_data_" + str(rospy.get_param("~vehicle_no"))), Float32, self.callback_imu) self.projector = lanelet2.projection.UtmProjector( lanelet2.io.Origin(rospy.get_param("/lat_origin"), rospy.get_param("/lon_origin"))) self.br = tf.TransformBroadcaster() self.pos = lanelet2.geometry.to2D( self.projector.forward( lanelet2.core.GPSPoint(rospy.get_param("/lat_origin"), rospy.get_param("/lon_origin")))) self.map = lanelet2.io.load(rospy.get_param("/map_file_name"), self.projector) self.yaw = 0 self.current_llet = (LaneletLayer.nearest(self.map.laneletLayer, self.pos, 1))[0] self.previous_llet = self.current_llet self.current_dist = distance(self.current_llet, self.pos) self.previous_dist = self.current_dist self.yield_lanlet = [] self.current_nos = len([ ll for ll in LaneletLayer.nearest(self.map.laneletLayer, self.pos, 4) if distance(ll, self.pos) == 0 ]) self.previous_nos = self.current_nos self.text = Marker() self.text.header.frame_id = "/map" self.text.scale.z = 1 self.text.id = 0 self.text.type = self.text.TEXT_VIEW_FACING self.text.action = self.text.ADD self.text.color.r = 0.65 self.text.color.g = 0.24 self.text.color.b = 0.25 self.text.color.a = 1 self.text.text = "Not inside a Lanelet"
def callback_gps(self, msg): #self.yaw = (rospy.wait_for_message("/imu_data", Float32)).data self.pos = lanelet2.geometry.to2D( self.projector.forward( lanelet2.core.GPSPoint(msg.latitude, msg.longitude))) msg.header.frame_id = str("/car_frame_" + str(rospy.get_param("~vehicle_no"))) self.pub.publish(msg) self.br.sendTransform( (self.pos.x, self.pos.y, 0), tf.transformations.quaternion_from_euler(0, 0, math.radians(self.yaw)), rospy.Time.now(), str("/car_frame_" + str(rospy.get_param("~vehicle_no"))), "/map") self.current_nos = len([ ll for ll in LaneletLayer.nearest(self.map.laneletLayer, self.pos, 4) if distance(ll, self.pos) == 0 ]) if self.current_nos: self.current_llet = ([ ll for ll in LaneletLayer.nearest(self.map.laneletLayer, self.pos, 4) if distance(ll, self.pos) == 0 ])[0] else: self.current_llet = (LaneletLayer.nearest(self.map.laneletLayer, self.pos, 1))[0] self.current_dist = distance(self.current_llet, self.pos) self.yield_lanlet = [] for el in self.current_llet.rightOfWay(): self.yield_lanlet = self.yield_lanlet + [ a.id for a in el.rightOfWayLanelets() ] self.print_msg() self.previous_llet = self.current_llet self.previous_dist = self.current_dist self.previous_nos = self.current_nos self.text.pose.position.x = self.pos.x self.text.pose.position.y = self.pos.y self.text.pose.position.z = 2 text = "" if len(self.yield_lanlet) and not self.current_dist: text = " Yield lanelets: " + str(self.yield_lanlet[0]) for ll in self.yield_lanlet[1:]: text = text + " && " + str(ll) temp = self.text.text self.text.text = self.text.text + text self.pub_text.publish(self.text) self.text.text = temp
def callback(msg): global map point = projector.forward(lanelet2.core.GPSPoint(msg.latitude, msg.longitude)) marker = Marker() marker.header.frame_id = "/map" marker.id = 0 marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 3 marker.scale.y = 3 marker.scale.z = 3 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = point.x marker.pose.position.y = point.y marker.pose.position.z = 0 publisher.publish(marker) p2d = lanelet2.geometry.to2D(point) x = LaneletLayer.nearest(map.laneletLayer, p2d, 1) if distance(x[0], p2d) == 0: print("You are in lanelet with ID: ", x[0].id) else: print("You are not inside a lanelet")
def get_current_lanelet(self, state, previous_lanelet=None): point = state.point radius = 3 bounding_box = BoundingBox2d( BasicPoint2d(point.x - radius, point.y - radius), BasicPoint2d(point.x + radius, point.y + radius)) nearby_lanelets = self.lanelet_map.laneletLayer.search(bounding_box) best_lanelet = None best_angle_diff = None best_dist = None best_can_pass = False for lanelet in nearby_lanelets: if self.traffic_rules.canPass(lanelet): dist_from_point = geometry.distance(lanelet, point) angle_diff = abs(self.angle_in_lane(state, lanelet)) can_pass = (False if previous_lanelet is None else self.can_pass(previous_lanelet, lanelet)) if (angle_diff < np.pi / 2 and (best_lanelet is None or (can_pass and not best_can_pass) or ((can_pass or not best_can_pass) and (dist_from_point < best_dist or (best_dist == dist_from_point and angle_diff < best_angle_diff))))): best_lanelet = lanelet best_angle_diff = angle_diff best_dist = dist_from_point best_can_pass = can_pass return best_lanelet
def show_path(self): self.marker_array.markers = [] i = 0 lanelet_list = [lanelet for lanelet in self.route] edges = [node for node in lanelet_list[0].centerline] flag = False for index, point in enumerate(edges[:-1]): if distance(point, self.vehicle_point_in_lanelet_cl) < distance( point, edges[index + 1]): flag = True self.marker_array.markers.append( get_a_marker(i, self.vehicle_point_in_lanelet_cl, edges[index + 1])) i = i + 1 for index_1, point_1 in enumerate(edges[index + 1:-1]): self.marker_array.markers.append( get_a_marker(i, point_1, edges[index_1 + index + 2])) i = i + 1 if flag: break for lanelet in lanelet_list[1:-1]: edges = [node for node in lanelet.centerline] for index, point in enumerate(edges[:-1]): self.marker_array.markers.append( get_a_marker(i, point, edges[index + 1])) i = i + 1 edges = [node for node in lanelet_list[-1].centerline] for index, point in enumerate(edges[:-1]): if distance(point, self.target_point_in_lanelet_cl) < distance( point, edges[index + 1]): self.marker_array.markers.append( get_a_marker(i, point, self.target_point_in_lanelet_cl)) break self.marker_array.markers.append( get_a_marker(i, point, edges[index + 1])) i = i + 1 self.pub.publish(del_marker_array) self.pub.publish(self.marker_array)
def callback_gps(self, msg): self.p2d = lanelet2.geometry.to2D( self.projector.forward( lanelet2.core.GPSPoint(msg.latitude, msg.longitude))) self.lanelet = self.find_lanelet() if distance(self.lanelet, self.p2d): self.pub.publish(del_marker_array) return 0 self.vehicle_point_in_lanelet_cl = self.vehicle_point_in_lanelet() self.routes_lines = self.find_routes_by_time(3) self.show_path()
def vehicle_in_front(cls, state, route, frame): vehicles_in_front = cls.get_vehicles_in_front(route, frame) min_dist = np.inf vehicle_in_front = None # find vehicle in front with closest distance for agent_id in vehicles_in_front: dist = geometry.distance(frame.agents[agent_id].point, state.point) if 0 < dist < min_dist: vehicle_in_front = agent_id min_dist = dist return vehicle_in_front, min_dist
def find_end_route(self, end_lanelet, end_dist_reqd): route_end = virt_cl() for point in end_lanelet.centerline: route_end.centerline.append(point) if length_point_list(route_end.centerline) > end_dist_reqd: break l1 = end_dist_reqd - length_point_list(route_end.centerline[:-1]) l2 = distance(route_end.centerline[-1], route_end.centerline[-2]) x1 = route_end.centerline[-2].x + (l1 / l2) * ( route_end.centerline[-1].x - route_end.centerline[-2].x) y1 = route_end.centerline[-2].y + (l1 / l2) * ( route_end.centerline[-1].y - route_end.centerline[-2].y) route_end.centerline[-1] = BasicPoint3d(x1, y1) return route_end
def callback_2(msg): global map, previous_lanelet_2, previous_dist_2 point = projector.forward( lanelet2.core.GPSPoint(msg.latitude, msg.longitude)) marker = Marker() marker.header.frame_id = "/map" marker.id = 0 marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 3 marker.scale.y = 3 marker.scale.z = 3 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = point.x marker.pose.position.y = point.y marker.pose.position.z = 0 publisher_2.publish(marker) p2d = lanelet2.geometry.to2D(point) current_lanelet = LaneletLayer.nearest(map.laneletLayer, p2d, 1) current_dist = distance(current_lanelet[0], p2d) if current_lanelet[0] == previous_lanelet_2[0] and ( (current_dist > 0 and previous_dist_2 > 0) or (current_dist == previous_dist_2)): shall_i_print = False else: shall_i_print = True previous_lanelet_2[0] = current_lanelet[0] previous_dist_2 = current_dist if current_dist == 0: if shall_i_print: print("V2 _ You are in lanelet with ID: ", current_lanelet[0].id) for el in current_lanelet[0].rightOfWay(): print "V2 _ Yield untill there are no vehicles in lanelets :", [ a.id for a in el.rightOfWayLanelets() ] else: if shall_i_print: if current_dist > 0.01: print("V2 _ You are not inside a lanelet") else: previous_dist_2 = 1000
def test_distance_llt2llt(self): self.assertEqual(distance(getLanelet(), getLanelet()), 0)
def test_distance_l2l(self): self.assertEqual(distance(getLineString(), getLineString()), 0)
def test_distance_basic_p2p(self): self.assertEqual(distance(getPoint().basicPoint(), getPoint()), 0)
def get_goal_routes(self, state: AgentState, goals): """ get most likely current lanelet and corresponding route for each goal lanelet must be: * close (radius 3m) * direction within 90 degrees of car (reduce this?) * can pass * goal is reachable from Rank lanelets based on: * distance to current point * angle diff """ point = state.point radius = 3 bounding_box = BoundingBox2d( BasicPoint2d(point.x - radius, point.y - radius), BasicPoint2d(point.x + radius, point.y + radius)) nearby_lanelets = [ l for l in self.lanelet_map.laneletLayer.search(bounding_box) if len(l.centerline) > 0 ] angle_diffs = [ abs(self.angle_in_lane(state, l)) for l in nearby_lanelets ] dists_from_point = [ geometry.distance(l, point) for l in nearby_lanelets ] # filter out invalid lanelets possible_lanelets = [] for idx, lanelet in enumerate(nearby_lanelets): if (angle_diffs[idx] < np.pi / 4 and dists_from_point[idx] < radius and self.traffic_rules.canPass(lanelet)): possible_lanelets.append(idx) # find best lanelet for each goal goal_lanelets = [] goal_routes = [] for goal in goals: # find reachable lanelets for each goal best_idx = None best_route = None for lanelet_idx in possible_lanelets: if (best_idx is None or dists_from_point[lanelet_idx] < dists_from_point[best_idx] or (dists_from_point[lanelet_idx] == dists_from_point[best_idx] and angle_diffs[lanelet_idx] < angle_diffs[best_idx])): lanelet = nearby_lanelets[lanelet_idx] route = self.route_to_goal(lanelet, goal) if route is not None: best_idx = lanelet_idx best_route = route if best_idx is None: goal_lanelet = None else: goal_lanelet = nearby_lanelets[best_idx] goal_lanelets.append(goal_lanelet) goal_routes.append(best_route) return goal_routes
def length_point_list(point_list): point_list_distance = 0 for i in range(len(point_list[:-1])): point_list_distance += distance(point_list[i], point_list[i + 1]) return point_list_distance
def print_msg(self): if self.current_dist == 0 and self.previous_dist == 0 and self.current_nos == 1 and self.previous_nos == 1 and self.current_llet == self.previous_llet: # remain inside same lanelet return 0 if self.current_dist == 0 and self.previous_dist == 0 and self.current_nos > 1 and self.previous_nos > 1 and self.current_nos == self.previous_nos: # remain in multiple lanelet return 0 if self.current_nos == 0 and self.previous_nos == 0: # remain outside lanelet return 0 if len(self.yield_lanlet): print "Yield untill there are no vehicles in lanelets ", self.yield_lanlet if self.current_dist == 0 and self.previous_dist == 0 and self.current_nos == 1 and self.previous_nos == 1 and not self.current_llet == self.previous_llet: # exactly one lane change print "1 You are in lanelet :", self.current_llet.id self.text.text = "Lanelet : " + str(self.current_llet.id) return 0 if self.current_dist == 0 and not self.previous_dist == 0: # came inside a lanelet print "2 You are in lanelet :", self.current_llet.id self.text.text = "Lanelet : " + str(self.current_llet.id) return 0 if self.current_dist == 0 and self.current_dist == 0 and self.previous_nos == 1 and self.current_nos > 1: # single to multiple x = [ ll.id for ll in LaneletLayer.nearest(self.map.laneletLayer, self.pos, 4) if distance(ll, self.pos) == 0 ] print "3 you are in one among the lanelets :", x text = "Lanelet : " for entry in x: text = text + str(entry) + " or " #print [ll.id for ll in LaneletLayer.nearest(self.map.laneletLayer, self.pos, 4) if distance(ll, self.pos) == 0] self.text.text = text[:-3] return 0 if self.current_dist == 0 and self.current_dist == 0 and self.previous_nos > 1 and self.current_nos == 1: # multiple to single print "4 You are in lanelet :", self.current_llet.id self.text.text = "Lanelet : " + str(self.current_llet.id) return 0 if self.current_dist == 0 and self.current_dist == 0 and self.previous_nos > 1 and self.current_nos > 1: # multple to multiple x = [ ll.id for ll in LaneletLayer.nearest(self.map.laneletLayer, self.pos, 4) if distance(ll, self.pos) == 0 ] print "5 you are in one among the lanelets :", x text = "Lanelet : " for entry in x: text = text + str(entry) + " or " #print [ll.id for ll in LaneletLayer.nearest(self.map.laneletLayer, self.pos, 4) if distance(ll, self.pos) == 0] self.text.text = text[:-3] return 0 if self.previous_dist == 0 and not self.current_dist == 0: # went outside lanelet print "6 You are not inside lanelet" self.text.text = "Not inside a Lanelet" return 0 print "none"