Beispiel #1
0
    def export(cls, file):
        """Export all the parking lines from the parking lines list."""
        for parkingLines in ParkingLines.list[:]:
            if len(parkingLines.ref) != 2:
                ParkingLines.list.remove(parkingLines)
                continue

            if WebotsObject.removalRadius > 0.0:
                # Check that the parking lines is inside the scope of a road,
                # otherwise, remove it from the parking lines list.
                if not Road.are_coords_close_to_some_road_waypoint(parkingLines.ref):
                    ParkingLines.list.remove(parkingLines)
                    continue

            # Coordinates extration
            c0 = (OSMCoord.coordDictionnary[parkingLines.ref[0]].x, OSMCoord.coordDictionnary[parkingLines.ref[0]].y, OSMCoord.coordDictionnary[parkingLines.ref[0]].z)
            c1 = (OSMCoord.coordDictionnary[parkingLines.ref[1]].x, OSMCoord.coordDictionnary[parkingLines.ref[1]].y, OSMCoord.coordDictionnary[parkingLines.ref[1]].z)

            # Compute the length and the angle
            v0 = Vector2D(c0[0], c0[2])
            v1 = Vector2D(c1[0], c1[2])
            deltaV = v0 - v1
            length = deltaV.norm()
            angle = - deltaV.angle() + math.pi
            expectedCarParkWidth = 2.4
            nCarParks = int((length + expectedCarParkWidth // 2) // expectedCarParkWidth)  # cf. http://stackoverflow.com/questions/3950372/round-with-integer-division
            carParkWidth = length / nCarParks

            file.write("ParkingLines {\n")
            file.write("  translation %.2lf %.2lf %.2lf\n" % (c0[0], c0[1] + 0.01, c0[2]))
            file.write("  rotation 0 1 0 %.2lf\n" % (angle))
            file.write("  numberOfCarParks %d\n" % (nCarParks))
            file.write("  carParkWidth %f\n" % (carParkWidth))
            file.write("}\n")
 def checkWaypoints(self):
     if self.wayPoints is not None:  
         # Compare distance between targetVector and currentPosition
         targetVector             = self.wayPoints[ self.activeWaypointIndex ]
         distance                 = Vector2D.fromPoints( self.position, targetVector )
         if distance.getVectorLength() < 10:
             
             self.activeWaypointIndex = (self.activeWaypointIndex + 1) % len( self.wayPoints )
             # Change to new direction
             targetVector             = self.wayPoints[ self.activeWaypointIndex ]
             # print "moving to next Waypoint " + str( targetVector )
             self.direction           = Vector2D.fromPoints( self.position, targetVector )
             self.direction.normalizeVector() 
 def setWaypointList(self, wpList):
     self.wayPoints = wpList
     # Recalc direction
     self.activeWaypointIndex = 0
     targetVector             = self.wayPoints[ self.activeWaypointIndex ]
     self.direction           = Vector2D.fromPoints( self.position, targetVector )
     self.direction.normalizeVector() 
    def handleMouseMovment(self, mousePos):
        if self.refToPlayer is not None:
            self.refToPlayer.setCrosshairPos(mousePos)

            if self.mouseButtonPressed is True:
                trgtVec = Vector2D(mousePos[0], mousePos[1])
                direction = direction = Vector2D.fromPoints(self.refToPlayer.position, trgtVec)
                direction.normalizeVector()
                self.refToPlayer.setMovment(direction, 20.0)
Beispiel #5
0
    def smooth_sharp_angles(self):
        """Smooth sharp angles from self."""
        if self.originalPath is None or not isinstance(
                self.originalPath,
                LineString) or len(self.originalPath.coords) < 3:
            return

        angleThreshold = 1.3  # angle threshlod in radian from which an angle is considered as sharp
        distanceThreshold = 5.0  # distance threshlod in meters from which a distance can be considered as sharp
        newPointsDistance = 3.0  # distance in meters where to add new points

        path = []
        path.append(
            [self.originalPath.coords[0][0], self.originalPath.coords[0][1]])
        for i in range(len(self.originalPath.coords) - 2):
            p1 = Vector2D(self.originalPath.coords[i][0],
                          self.originalPath.coords[i][1])
            p2 = Vector2D(self.originalPath.coords[i + 1][0],
                          self.originalPath.coords[i + 1][1])
            p3 = Vector2D(self.originalPath.coords[i + 2][0],
                          self.originalPath.coords[i + 2][1])

            v1to2 = p2 - p1
            v2to3 = p3 - p2

            angle = v1to2.angle(v2to3)

            if abs(angle) > angleThreshold and v1to2.norm(
            ) > distanceThreshold and v2to3.norm() > distanceThreshold:
                # sharp angle detected in p2
                p2before = p2 + v1to2.normalize() * (-newPointsDistance)
                p2after = p2 + v2to3.normalize() * newPointsDistance
                p2new = ((p2before + p2after) * 0.5 + p2) * 0.5

                path.append([p2before.x, p2before.y])
                path.append([p2new.x, p2new.y])
                path.append([p2after.x, p2after.y])
            else:
                path.append([p2.x, p2.y])
        path.append(
            [self.originalPath.coords[-1][0], self.originalPath.coords[-1][1]])

        self.originalPath = LineString(path)
        self.finalPath = self.originalPath
Beispiel #6
0
def convert_polygon_to_vector2d_list(polygon):
    """Convert a shapely polygon to a list of Vector2D."""
    assert isinstance(polygon, Polygon) or isinstance(polygon, MultiPolygon)
    coords = []
    if isinstance(polygon, Polygon):
        coords = polygon.exterior.coords
    elif isinstance(polygon, MultiPolygon):
        for geom in polygon.geoms:
            coords += geom.exterior.coords
    else:
        return None
    return [Vector2D(x, y) for (x, y) in coords]
Beispiel #7
0
def simplify_polygon(polygon, tolerance=0.01):
    """Remove doubles coords from a polygon."""
    assert isinstance(polygon, Polygon) or isinstance(polygon, MultiPolygon)
    # Get the coordinates
    coords = []
    if isinstance(polygon, Polygon):
        coords = polygon.exterior.coords
    elif isinstance(polygon, MultiPolygon):
        for geom in polygon.geoms:
            coords += geom.exterior.coords
    else:
        return None
    # remove the doubled coordinates
    newCoords = []
    v0 = Vector2D(float('inf'), float('inf'))
    for coord in coords:
        v = Vector2D(coord[0], coord[1])
        if (v0 - v).norm() > tolerance:
            v0 = v
            newCoords += [[coord[0], coord[1]]]
    return Polygon(newCoords)
Beispiel #8
0
def intersects(a1, b1, a2, b2, tolerance=0.05):
    """Compute the intersection of a segment a1, b1 with a segment a2, b2. a1, a2, b1 and b2 are vectors. Returns a Vector2D."""
    # src: http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
    # Note: Almost the same can be achieved using LineString.intersection(), but the coplanar bounds are
    #       rejected. For this reason, a custom algorithm is preferred there.

    assert isinstance(a1, Vector2D)
    assert isinstance(b1, Vector2D)
    assert isinstance(a2, Vector2D)
    assert isinstance(b2, Vector2D)

    ls1 = LineString([[a1.x, a1.y], [b1.x, b1.y]]).buffer(tolerance)
    ls2 = LineString([[a2.x, a2.y], [b2.x, b2.y]]).buffer(tolerance)

    intersection = ls1.intersection(ls2)
    if intersection is not None and len(intersection.centroid.coords) == 1:
        return Vector2D(intersection.centroid.coords[0][0], intersection.centroid.coords[0][1])

    return None
Beispiel #9
0
    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)
Beispiel #10
0
    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