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"
