def cut_roads(self):
"""Remove the roads touching this junction using the junction shape and
compute the start/end angles at these intersections."""
if self.shape is None:
if len(self.roads) == 2:
# When there is a crossroad containing 2 similar roads and
# the crossroad has no shape, the roads start/end angles should match.
r0 = list(self.roads)[0]
r1 = list(self.roads)[1]
if r0.is_similar(r1) and len(
r0.originalPath.coords) >= 2 and len(
r1.originalPath.coords) >= 2:
v00 = None
v01 = None
if r0.startJunction == self:
v00 = Vector2D(r0.originalPath.coords[0][0],
r0.originalPath.coords[0][1])
else:
v00 = Vector2D(r0.originalPath.coords[-1][0],
r0.originalPath.coords[-1][1])
if r0.startJunction == self:
v01 = Vector2D(r0.originalPath.coords[1][0],
r0.originalPath.coords[1][1])
else:
v01 = Vector2D(r0.originalPath.coords[-2][0],
r0.originalPath.coords[-2][1])
alpha = (v00 - v01).angle()
if r0.startJunction == self:
r0.startingAngle = alpha + 0.5 * math.pi
elif r0.endJunction == self:
r0.endingAngle = alpha - 0.5 * math.pi
if r1.startJunction == self:
r1.startingAngle = alpha - 0.5 * math.pi
elif r1.endJunction == self:
r1.endingAngle = alpha + 0.5 * math.pi
return
for road in self.roads:
# Substract this crossroad shape to the road path
road.shape = road.shape.difference(self.shape)
if not isinstance(road.finalPath, LineString):
road.finalPath = road.finalPath.difference(self.shape)
continue
roadPathBeforeEndsRemoval = [
Vector2D(x, y) for (x, y) in road.finalPath.coords
]
firstPoint = roadPathBeforeEndsRemoval[0]
lastPoint = roadPathBeforeEndsRemoval[-1]
road.finalPath = road.finalPath.difference(self.shape)
if not isinstance(road.finalPath,
LineString) or not road.finalPath.coords:
continue
roadPath = [Vector2D(x, y) for (x, y) in road.finalPath.coords]
# Search if the first or last point are still present in the cut path
firstPointPresent = False
lastPointPresent = False
for c in road.finalPath.coords:
v = Vector2D(c[0], c[1])
if (v - firstPoint).norm() < 0.1:
firstPointPresent = True
elif (v - lastPoint).norm() < 0.1:
lastPointPresent = True
# Compute the starting and ending angles thanks to the intersection
# point between the crossroad shape and the road path.
crossroadCoords = convert_polygon_to_vector2d_list(self.shape)
if not firstPointPresent:
roadSegment = (roadPath[1], roadPath[0])
for i in range(len(crossroadCoords) - 1):
crossroadShapeSegment = (crossroadCoords[i],
crossroadCoords[i + 1])
intersection = intersects(roadSegment[0], roadSegment[1],
crossroadShapeSegment[0],
crossroadShapeSegment[1])
if intersection and intersection != roadPath[1]:
road.startingAngle = (
crossroadShapeSegment[1] -
crossroadShapeSegment[0]).angle() % (2.0 * math.pi)
if not lastPointPresent:
nRoadPath = len(roadPath)
roadSegment = (roadPath[nRoadPath - 2],
roadPath[nRoadPath - 1])
for i in range(len(crossroadCoords) - 1):
crossroadShapeSegment = (crossroadCoords[i],
crossroadCoords[i + 1])
intersection = intersects(roadSegment[0], roadSegment[1],
crossroadShapeSegment[0],
crossroadShapeSegment[1])
if intersection and intersection != roadPath[nRoadPath -
2]:
road.endingAngle = (
crossroadShapeSegment[0] -
crossroadShapeSegment[1]).angle() % (2.0 * math.pi)
def export(cls, f):
"""Export all the roads from the roads list."""
# Export crossroads
sortedCrossroads = sorted(cls.crossroads.values(), key=lambda x: x.id)
for crossroad in sortedCrossroads:
translation = Vector2D(
OSMCoord.coordDictionnary[crossroad.nodeRef].x,
OSMCoord.coordDictionnary[crossroad.nodeRef].z)
f.write('Crossroad {\n')
f.write(' translation %f %f %f\n' %
(translation.x, vOffset, translation.y))
if crossroad.name is not None:
name = crossroad.name
i = 1
while name in Crossroad.nameList:
name = crossroad.name + '(%d)' % i
i += 1
Crossroad.nameList.append(name)
f.write(' name "%s"\n' % name)
else:
f.write(' name "crossroad(%d)"\n' % Crossroad.nameIndex)
Crossroad.nameIndex += 1
f.write(' id "%s"\n' % crossroad.id)
f.write(' shape [\n')
if crossroad.shape is not None:
coords = convert_polygon_to_vector2d_list(crossroad.shape)
for coord in coords:
height = 0
if WebotsObject.enable3D:
osmCoord = cls._convert_webots_to_osm_coord(
[coord.x, coord.y])
height += WebotsObject.elevation.interpolate_height(
osmCoord[0], osmCoord[1])
f.write(' %f %f %f\n' %
(coord.x - translation.x, height,
coord.y - translation.y))
f.write(' ]\n')
f.write(' connectedRoadIDs [\n')
sortedRoads = sorted(crossroad.roads, key=lambda x: x.id)
for road in sortedRoads:
f.write(' "%s"\n' % (road.id))
f.write(' ]\n')
f.write('}\n')
# Export roads
for road in cls.roads:
if not isinstance(road.finalPath,
LineString) or not road.finalPath.coords:
continue
coords = road.finalPath.coords
f.write('Road {\n')
f.write(' translation %f %f %f\n' %
(coords[0][0], vOffset +
road.layer * WebotsObject.layerHeight, coords[0][1]))
if road.streetName:
name = road.streetName
i = 1
while name in Road.nameList:
name = road.streetName + '(%d)' % i
i += 1
Road.nameList.append(name)
f.write(' name "%s"\n' % name.replace('"', ''))
else:
f.write(' name "road(%d)"\n' % Road.roadNameIndex)
Road.roadNameIndex += 1
f.write(' id "%s"\n' % road.id)
if road.startJunction is not None:
f.write(' startJunction "%s"\n' % road.startJunction.id)
if road.endJunction is not None:
f.write(' endJunction "%s"\n' % road.endJunction.id)
f.write(' splineSubdivision 0\n')
f.write(' numberOfLanes %d\n' % road.lanes)
f.write(' numberOfForwardLanes %d\n' % road.forwardLanes)
if road.maxSpeed > 0.0:
f.write(' speedLimit %f\n' % road.maxSpeed)
f.write(' width %f\n' % road.width)
if road.noBorder:
f.write(' leftBorder FALSE\n')
f.write(' rightBorder FALSE\n')
if road.startingAngle:
if road.startingAngle > math.pi:
road.startingAngle -= 2 * math.pi
elif road.startingAngle < -math.pi:
road.startingAngle += 2 * math.pi
f.write(' startingAngle %f\n' % road.startingAngle)
if road.endingAngle:
if road.endingAngle > math.pi:
road.endingAngle -= 2 * math.pi
elif road.endingAngle < -math.pi:
road.endingAngle += 2 * math.pi
f.write(' endingAngle %f\n' % road.endingAngle)
f.write(' lines [\n')
for i in range(road.lanes - 1):
f.write(' RoadLine {\n')
f.write(' type "%s"\n' %
('continuous' if
(i == road.forwardLanes - 1) else 'dashed'))
f.write(' }\n')
f.write(' ]\n')
if road.turnLanesForward:
f.write(' turnLanesForward "%s"\n' % road.turnLanesForward)
if road.turnLanesBackward:
f.write(' turnLanesBackward "%s"\n' % road.turnLanesBackward)
if not Road.noIntersectionRoadLines and not road.insideARoundAbout:
if road.startJunction is not None and len(
road.startJunction.roads) > 2:
f.write(' startLine [\n')
for l in range(road.forwardLanes):
f.write(' "textures/road_line_dashed.png"\n')
for l in range(road.backwardLanes):
f.write(' "textures/road_line_triangle.png"\n')
f.write(' ]\n')
if road.endJunction is not None and len(
road.endJunction.roads) > 2:
f.write(' endLine [\n')
for l in range(road.forwardLanes):
f.write(' "textures/road_line_triangle.png"\n')
for l in range(road.backwardLanes):
f.write(' "textures/road_line_dashed.png"\n')
f.write(' ]\n')
f.write(' wayPoints [\n')
for coord in coords:
height = 0
if WebotsObject.enable3D:
osmCoord = cls._convert_webots_to_osm_coord(coord)
height += WebotsObject.elevation.interpolate_height(
osmCoord[0], osmCoord[1])
f.write(
' %f %f %f\n' %
(coord[0] - coords[0][0], height, coord[1] - coords[0][1]))
Road.allRoadWayPoints.append([coord[0], coord[1]])
f.write(' ]\n')
f.write('}\n')
# Export pedestrian crossings
for crossingNode in road.crossings:
for i in range(len(road.refs)):
ref = road.refs[i]
translation = Vector2D(
OSMCoord.coordDictionnary[crossingNode.OSMID].x,
OSMCoord.coordDictionnary[crossingNode.OSMID].z)
if (translation.x == OSMCoord.coordDictionnary[ref].x and
translation.y == OSMCoord.coordDictionnary[ref].z):
if i == len(road.refs) - 1:
otherRef = road.refs[i - 1]
else:
otherRef = road.refs[i + 1]
alpha = math.atan(
(OSMCoord.coordDictionnary[otherRef].x -
OSMCoord.coordDictionnary[ref].x) /
(OSMCoord.coordDictionnary[otherRef].z -
OSMCoord.coordDictionnary[ref].z))
width = road.width
for crossroad in cls.crossroads.values():
# If the pedestrian crossing is on crossroad, it is moved.
if (translation.x == OSMCoord.coordDictionnary[
crossroad.nodeRef].x
and translation.y == OSMCoord.
coordDictionnary[crossroad.nodeRef].z):
# Crossing must be parallel to this vector.
vector = Vector2D(
OSMCoord.coordDictionnary[otherRef].x -
OSMCoord.coordDictionnary[ref].x,
OSMCoord.coordDictionnary[otherRef].z -
OSMCoord.coordDictionnary[ref].z)
normVector = math.sqrt(vector.x**2 +
vector.y**2)
distanceFromCenter = 6.0 # distance to add to the translation in the good direction
if crossroad.shape is not None:
bounds = crossroad.shape.bounds
# center of the crossroad
centre = Vector2D(
(bounds[2] + bounds[0]) / 2,
(bounds[3] + bounds[1]) / 2)
# difference between crossroad point and center
difference = Vector2D(
centre.x - OSMCoord.coordDictionnary[
crossroad.nodeRef].x,
centre.y - OSMCoord.coordDictionnary[
crossroad.nodeRef].z)
beta = math.atan(difference.x /
difference.y)
normDifference = math.sqrt(
difference.x**2 + difference.y**2)
# radius of the crossroad
radius = math.sqrt((
(bounds[2] - bounds[0]) / 2)**2 + (
(bounds[3] - bounds[1]) / 2)**2)
# We need to add a correction distance to crossing translation because the center of the
# crossroad is not the same point as the crossroad point.
corrector = normDifference * abs(
math.cos(beta - alpha))
if normVector > math.sqrt(
(centre.x - OSMCoord.
coordDictionnary[otherRef].x)**2 +
(centre.y - OSMCoord.
coordDictionnary[otherRef].z)**2):
corrector = -corrector
distanceFromCenter = radius - corrector
translation = Vector2D(
OSMCoord.coordDictionnary[ref].x +
(distanceFromCenter / normVector *
vector.x),
OSMCoord.coordDictionnary[ref].z +
(distanceFromCenter / normVector *
vector.y))
break
f.write('PedestrianCrossing {\n')
f.write(' translation %f %f %f\n' %
(translation.x, -0.07, translation.y))
f.write(' rotation 0 1 0 %f\n' % (alpha))
f.write(' name "pedestrian crossing(%d)"\n' %
(Road.pedestrianCrossingNameIndex))
Road.pedestrianCrossingNameIndex += 1
f.write(' size %f %f\n' % (width, width * 0.4))
f.write('}\n')
break