def createReferenceBorder(planView, laneOffsets):
""" Create the first (most inner) border line for a road, includes the lane Offsets """
firstLaneBorder = Border()
# Set reference to plan view
firstLaneBorder.reference = planView
firstLaneBorder.refOffset = 0.0
# Lane offfsets will be coeffs
if any(laneOffsets):
for laneOffset in laneOffsets:
firstLaneBorder.coeffsOffsets.append(laneOffset.sPos)
firstLaneBorder.coeffs.append(laneOffset.coeffs)
else:
firstLaneBorder.coeffsOffsets.append(0.0)
firstLaneBorder.coeffs.append([0.0])
return firstLaneBorder
def laneSectionToPLanes(laneSection, referenceBorder):
""" Convert a whole lane section into a list of planes """
newPLanes = []
# Length of this lane section
laneSectionStart = laneSection.sPos
for side in ["right", "left"]:
# lanes loaded by opendriveparser are aleady sorted by id
# coeffsFactor decides if border is left or right of the reference line
if side == "right":
lanes = laneSection.rightLanes
coeffsFactor = -1.0
else:
lanes = laneSection.leftLanes
coeffsFactor = 1.0
# Most inner border
laneBorders = [referenceBorder]
prevInnerNeighbours = []
for lane in lanes:
if abs(lane.id) > 1:
if lane.id > 0:
innerLaneId = lane.id - 1
outerLaneId = lane.id + 1
else:
innerLaneId = lane.id + 1
outerLaneId = lane.id - 1
innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1)
innerNeighbourSameDirection = True
outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1)
else:
# Skip lane id 0
if lane.id == 1:
innerLaneId = -1
outerLaneId = 2
else:
innerLaneId = 1
outerLaneId = -2
innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1)
innerNeighbourSameDirection = False
outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1)
innerNeighbours = []
# Create outer lane border
newPLaneBorder = Border()
newPLaneBorder.reference = laneBorders[-1]
if len(laneBorders) == 1:
newPLaneBorder.refOffset = laneSectionStart
else:
# Offset from reference border is already included in first created outer lane border
newPLaneBorder.refOffset = 0.0
for width in lane.widths:
newPLaneBorder.coeffsOffsets.append(width.sOffset)
newPLaneBorder.coeffs.append([x * coeffsFactor for x in width.coeffs])
laneBorders.append(newPLaneBorder)
# Create new lane for each width segment
newPLanesList = []
for width in lane.widths:
newPLane = PLane(
id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, width.idx),
type=lane.type
)
if allCloseToZero(width.coeffs):
newPLane.isNotExistent = True
newPLane.innerNeighbours = prevInnerNeighbours
newPLane.length = width.length
newPLane.innerBorder = laneBorders[-2]
newPLane.innerBorderOffset = width.sOffset + laneBorders[-1].refOffset
newPLane.outerBorder = laneBorders[-1]
newPLane.outerBorderOffset = width.sOffset
newPLanesList.append(newPLane)
innerNeighbours.append(newPLane)
# Organize everything in a pLaneGroup if it is more than one element
if len(newPLanesList) >= 1:
newPLaneGroup = PLaneGroup(
id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, -1),
innerNeighbour=innerNeighbourId,
innerNeighbourSameDirection=innerNeighbourSameDirection,
outerNeighbour=outerNeighbourId,
reverse=True if lane.id > 0 else False
)
for newPLane in newPLanesList:
newPLaneGroup.append(newPLane)
newPLanes.append(newPLaneGroup)
else:
newPLanes.append(newPLanesList[0])
prevInnerNeighbours = innerNeighbours
return newPLanes