def part4reading_and_writing():
# there are two ways of loading/writing a lanelet map: a robust one and an normal one. The robust one returns found
# issues as extra return parameter
map = LaneletMap()
lanelet = get_a_lanelet()
map.add(lanelet)
path = os.path.join(tempfile.mkdtemp(), 'mapfile.osm')
projector = UtmProjector(lanelet2.io.Origin(49, 8.4))
lanelet2.io.write(path, map, projector)
mapLoad, errors = lanelet2.io.loadRobust(path, projector)
assert not errors
assert mapLoad.laneletLayer.exists(lanelet.id)
def part6routing():
# and this as well
projector = UtmProjector(lanelet2.io.Origin(49, 8.4))
map = lanelet2.io.load(example_file, projector)
traffic_rules = lanelet2.traffic_rules.create(lanelet2.traffic_rules.Locations.Germany,
lanelet2.traffic_rules.Participants.Vehicle)
graph = lanelet2.routing.RoutingGraph(map, traffic_rules)
lanelet = map.laneletLayer[4984315]
toLanelet = map.laneletLayer[2925017]
assert graph.following(lanelet)
assert len(graph.reachableSet(lanelet, 100, 0)) > 10
assert len(graph.possiblePaths(lanelet, 100, 0, False)) == 1
# here we query a route through the lanelets and get all the vehicle lanelets that conflict with the shortest path
# in that route
route = graph.getRoute(lanelet, toLanelet)
path = route.shortestPath()
confLlts = [llt for llt in route.allConflictingInMap() if llt not in path]
assert len(confLlts) > 0
#!/usr/bin/env python
from visualization_msgs.msg import Marker
from sensor_msgs.msg import NavSatFix
import rospy
from lanelet2.projection import UtmProjector
import lanelet2
from lanelet2.core import LaneletLayer
from lanelet2.geometry import distance
publisher = rospy.Publisher('gps_marker', Marker, queue_size=10)
rospy.init_node('gps_marker')
projector = UtmProjector(
lanelet2.io.Origin(rospy.get_param("/lat_origin"),
rospy.get_param("/lon_origin")))
map = lanelet2.io.load(rospy.get_param("/map_file_name"), projector)
def callback(msg):
global map, previous_lanelet, previous_dist
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
