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)
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
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]
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)
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
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
