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