def getAngleBetweenStraightRoads(road1: ExtendedRoad, road2: ExtendedRoad):
"""roads must be already adjusted
Args:
road1 (ExtendedRoad): [description]
road2 (ExtendedRoad): [description]
"""
x1Start, y1Start, _ = road1.getAdjustedStartPosition()
x1End, y1End, _ = road1.getAdjustedEndPosition()
x2Start, y2Start, _ = road2.getAdjustedStartPosition()
x2End, y2End, _ = road2.getAdjustedEndPosition()
angle1 = math.atan((y1End - y1Start) / (x1End - x1Start))
angle2 = math.atan((y2End - y2Start) / (x2End - x2Start))
if angle1 < 0:
angle1 = angle1 + np.pi
if angle2 < 0:
angle2 = angle2 + np.pi
print(f"angle1 {math.degrees(angle1)}")
print(
f"angle2 {math.degrees(angle2)} start { x2Start, y2Start } end {x2End, y2End}"
)
return angle2 - angle1
def createCurveByLength(self,
roadId,
length,
isJunction=False,
curvature=StandardCurvature.Medium.value):
junction = self.getJunctionSelection(isJunction)
n_lanes = 1
lane_offset = 3
pv = ExtendedPlanview()
arc = pyodrx.Arc(curvature, length=length)
pv.add_geometry(arc)
# create lanes
lsec = pyodrx.LaneSection(0, pyodrx.standard_lane())
for _ in range(1, n_lanes + 1, 1):
lsec.add_right_lane(pyodrx.standard_lane(lane_offset))
lsec.add_left_lane(pyodrx.standard_lane(lane_offset))
laneSections = extensions.LaneSections()
laneSections.add_lanesection(lsec)
# create road
road = ExtendedRoad(roadId, pv, laneSections, road_type=junction)
road.curveType = StandardCurveTypes.S
return road
def createCurveGeoAndLanes(self,
roadId,
isJunction,
curvature,
arcAngle,
clothAngle,
n_lanes,
lane_offset,
laneSides=LaneSides.BOTH,
isLeftTurnLane=False,
isRightTurnLane=False,
isLeftMergeLane=False,
isRightMergeLane=False):
junction = extensions.getJunctionSelection(isJunction)
pv = self.createPVForArcWithCloths(curvature, arcAngle, clothAngle)
length = pv.getTotalLength()
laneSections = self.laneBuilder.getStandardLanes(
n_lanes,
lane_offset,
laneSides,
roadLength=length,
isLeftTurnLane=isLeftTurnLane,
isRightTurnLane=isRightTurnLane,
isLeftMergeLane=isLeftMergeLane,
isRightMergeLane=isRightMergeLane)
road = ExtendedRoad(roadId,
pv,
laneSections,
road_type=junction,
curveType=StandardCurveTypes.LongArc)
return road
def createWithRightTurnLanesOnLeft(
self,
roadId,
length=100,
junction=-1,
n_lanes=1,
lane_offset=3,
laneSides=LaneSides.BOTH,
isLeftTurnLane=False,
isRightTurnLane=False,
isLeftMergeLane=False,
isRightMergeLane=False,
numberOfRightTurnLanesOnLeft=1,
mergeLaneOnTheOppositeSideForInternalTurn=True):
# create geometry
pv = self.createPVForLine(length)
laneSections = self.laneBuilder.getStandardLanesWithInternalTurns(
n_lanes,
lane_offset,
laneSides,
roadLength=length,
isLeftTurnLane=isLeftTurnLane,
isRightTurnLane=isRightTurnLane,
isLeftMergeLane=isLeftMergeLane,
isRightMergeLane=isRightMergeLane,
numberOfRightTurnLanesOnLeft=numberOfRightTurnLanesOnLeft,
mergeLaneOnTheOppositeSideForInternalTurn=
mergeLaneOnTheOppositeSideForInternalTurn)
road = ExtendedRoad(roadId, pv, laneSections, road_type=junction)
return road
def getContactPoints(road1: ExtendedRoad, road2: ExtendedRoad):
# TODO we are assuming start points
road1Cp = None
road2Cp = None
# if road1 is a pred, then road1's cp and start
road1IsPred = road2.getExtendedPredecessorByRoadId(road1.id)
if road1IsPred is not None:
road1Cp = road1IsPred.cp
# if road2 is a pred, then road2's cp and start
road2IsPred = road1.getExtendedPredecessorByRoadId(road2.id)
if road2IsPred is not None:
road2Cp = road2IsPred.cp # road1's start is connected to road2's cp
road2IsSuc = road1.getExtendedSuccessorByRoadId(road2.id)
if road2IsSuc is not None:
road2Cp = road2IsSuc.cp # road1's end is connected to road2's cp
road1IsSuc = road2.getExtendedSuccessorByRoadId(road1.id)
if road1IsSuc is not None:
road1Cp = road1IsSuc.cp # road1's cp is connected to road2's start
# if road1IsPred is not None:
# return road1IsPred.cp, pyodrx.ContactPoint.start # road1's cp is connected to road2's start
# # if road2 is a pred, then road2's cp and start
# road2IsPred = road1.getExtendedPredecessorByRoadId(road2.id)
# if road2IsPred is not None:
# return pyodrx.ContactPoint.start, road2IsPred.cp # road1's start is connected to road2's cp
# road2IsSuc = road1.getExtendedSuccessorByRoadId(road2.id)
# if road2IsSuc is not None:
# return pyodrx.ContactPoint.end, road2IsSuc.cp # road1's end is connected to road2's cp
# road1IsSuc = road2.getExtendedSuccessorByRoadId(road1.id)
# if road1IsSuc is not None:
# return road1IsSuc.cp, pyodrx.ContactPoint.end # road1's cp is connected to road2's start
if road1Cp is None or road2Cp is None:
raise Exception(
f"contact points not available for {road1.id} and {road2.id}")
return road1Cp, road2Cp
def createWithLeftTurnLanesOnRight(
self,
roadId,
length=100,
junction=-1,
n_lanes=1,
lane_offset=3,
laneSides=LaneSides.BOTH,
isLeftTurnLane=False,
isRightTurnLane=False,
isLeftMergeLane=False,
isRightMergeLane=False,
numberOfLeftTurnLanesOnRight=1,
mergeLaneOnTheOppositeSideForInternalTurn=True):
"""Will create numberOfLeftTurnLanesOnRight left turn lanes on the right side of the center line. Equal number of mergelanes will be created on the left side of the center lane, too.
Args:
roadId ([type]): [description]
length (int, optional): [description]. Defaults to 100.
junction (int, optional): [description]. Defaults to -1.
n_lanes (int, optional): [description]. Defaults to 1.
lane_offset (int, optional): [description]. Defaults to 3.
laneSides ([type], optional): [description]. Defaults to LaneSides.BOTH.
isLeftTurnLane (bool, optional): [description]. Defaults to False.
isRightTurnLane (bool, optional): [description]. Defaults to False.
isLeftMergeLane (bool, optional): [description]. Defaults to False.
isRightMergeLane (bool, optional): [description]. Defaults to False.
numberOfLeftTurnLanesOnRight (int, optional): [description]. Defaults to 1.
Returns:
[type]: [description]
"""
# create geometry
pv = self.createPVForLine(length)
laneSections = self.laneBuilder.getStandardLanesWithInternalTurns(
n_lanes,
lane_offset,
laneSides,
roadLength=length,
isLeftTurnLane=isLeftTurnLane,
isRightTurnLane=isRightTurnLane,
isLeftMergeLane=isLeftMergeLane,
isRightMergeLane=isRightMergeLane,
numberOfLeftTurnLanesOnRight=numberOfLeftTurnLanesOnRight,
mergeLaneOnTheOppositeSideForInternalTurn=
mergeLaneOnTheOppositeSideForInternalTurn)
road = ExtendedRoad(roadId, pv, laneSections, road_type=junction)
return road
def createParamPoly3(self,
roadId,
isJunction=False,
au=0,
bu=20,
cu=20,
du=10,
av=0,
bv=2,
cv=20,
dv=10,
prange='normalized',
length=None):
junction = self.getJunctionSelection(isJunction)
n_lanes = 1
lane_offset = 3
pv = ExtendedPlanview()
poly = pyodrx.ParamPoly3(au, bu, cu, du, av, bv, cv, dv, prange,
length)
pv.add_geometry(poly)
# create lanes
lsec = pyodrx.LaneSection(0, pyodrx.standard_lane())
for _ in range(1, n_lanes + 1, 1):
lsec.add_right_lane(pyodrx.standard_lane(lane_offset))
lsec.add_left_lane(pyodrx.standard_lane(lane_offset))
laneSections = extensions.LaneSections()
laneSections.add_lanesection(lsec)
# create road
road = ExtendedRoad(roadId, pv, laneSections, road_type=junction)
road.curveType = StandardCurveTypes.S
return road
def createParamPoly3(self,
roadId,
isJunction=False,
au=0,
bu=20,
cu=20,
du=10,
av=0,
bv=2,
cv=20,
dv=10,
prange='normalized',
length=None,
n_lanes=1,
lane_offset=3,
laneSides=LaneSides.BOTH,
isLeftTurnLane=False,
isRightTurnLane=False,
isLeftMergeLane=False,
isRightMergeLane=False):
junction = self.getJunctionSelection(isJunction)
pv = ExtendedPlanview()
poly = pyodrx.ParamPoly3(au, bu, cu, du, av, bv, cv, dv, prange,
length)
# poly = extensions.IntertialParamPoly(au,bu,cu,du,av,bv,cv,dv,prange,length)
pv.add_geometry(poly)
length = pv.getTotalLength()
laneSections = self.laneBuilder.getStandardLanes(
n_lanes,
lane_offset,
laneSides,
roadLength=length,
isLeftTurnLane=isLeftTurnLane,
isRightTurnLane=isRightTurnLane,
isLeftMergeLane=isLeftMergeLane,
isRightMergeLane=isRightMergeLane)
# create lanes
road = ExtendedRoad(roadId,
pv,
laneSections,
road_type=junction,
curveType=StandardCurveTypes.Poly)
return road
def create(self,
roadId,
n_lanes_left=1,
n_lanes_right=1,
length=20,
junction=-1,
lane_offset=3,
laneSides=LaneSides.BOTH,
numLeftTurnsOnLeft=0,
numRightTurnsOnRight=0,
numLeftMergeOnLeft=0,
numRightMergeOnRight=0,
numberOfLeftTurnLanesOnRight=0,
numberOfRightTurnLanesOnLeft=0,
mergeLaneOnTheOppositeSideForInternalTurn=True,
force3Section=False):
# create geometry
pv = self.createPVForLine(length)
# laneSections = self.laneBuilder.getStandardLanes(n_lanes, lane_offset, laneSides,
# roadLength=length,
# isLeftTurnLane=isLeftTurnLane, isRightTurnLane=isRightTurnLane,
# isLeftMergeLane=isLeftMergeLane, isRightMergeLane=isRightMergeLane)
singleSide = False
if laneSides != LaneSides.BOTH:
singleSide = True
laneSections = self.laneBuilder.getLanes(
n_lanes_left,
n_lanes_right,
lane_offset=lane_offset,
singleSide=singleSide,
roadLength=length,
numLeftTurnsOnLeft=numLeftTurnsOnLeft,
numRightTurnsOnRight=numRightTurnsOnRight,
numLeftMergeOnLeft=numLeftMergeOnLeft,
numRightMergeOnRight=numRightMergeOnRight,
numberOfLeftTurnLanesOnRight=numberOfLeftTurnLanesOnRight,
numberOfRightTurnLanesOnLeft=numberOfRightTurnLanesOnLeft,
mergeLaneOnTheOppositeSideForInternalTurn=
mergeLaneOnTheOppositeSideForInternalTurn,
force3Section=force3Section)
road = ExtendedRoad(roadId, pv, laneSections, road_type=junction)
return road
def createLanesForConnectionRoad(self, connectionRoad: ExtendedRoad,
predRoad: ExtendedRoad,
sucRoad: ExtendedRoad,
strategy = LaneConfigurationStrategies.MERGE_EDGE,
countryCode=extensions.CountryCodes.US):
"""Assumes start of connection road is connected to predRoad and end to sucRoad and connection road's lanes are not connected to either of the roads.
It can connect roads with two different lane configurations.
Args:
connectionRoad (ExtendedRoad):
predRoad (ExtendedRoad): Extended predecessor road of connectionRoad. That means connection road's start is connected to predRoad
sucRoad (ExtendedRoad): Extended successor road of connectionRoad. That means connection road's end is connected to sucRoad
strategy ([type], optional): [description]. Defaults to LaneConfigurationStrategies.MERGE_EDGE.
"""
try:
cp1, cp1Con = RoadLinker.getContactPoints(predRoad, connectionRoad)
cp2, cp2Con = RoadLinker.getContactPoints(sucRoad, connectionRoad)
laneSection1 = predRoad.getLaneSectionByCP(cp1)
laneSection2 = sucRoad.getLaneSectionByCP(cp2)
connectionRoad.clearLanes()
leftConnections, rightConnections = LaneConfiguration.getLaneLinks(laneSection1, laneSection2, (cp1 == cp2), strategy)
# now we need to workout the number of straight lanes, merge lanes, and turn lanes on each side.
# switch lane sides if cp1 and cp1Con are the same, because the lane orientation is reversed
if cp1 == cp1Con:
leftNumStandard, leftNumMerge, leftNumTurn = LaneConfiguration.getNumberDifferentLanes(rightConnections)
rightNumStandard, rightNumMerge, rightNumTurn = LaneConfiguration.getNumberDifferentLanes(leftConnections)
else:
leftNumStandard, leftNumMerge, leftNumTurn = LaneConfiguration.getNumberDifferentLanes(leftConnections)
rightNumStandard, rightNumMerge, rightNumTurn = LaneConfiguration.getNumberDifferentLanes(rightConnections)
connectionRoad.lanes = self.getLanes(n_lanes_left=leftNumStandard, n_lanes_right=rightNumStandard,
roadLength=connectionRoad.length(),
numLeftTurnsOnLeft=leftNumTurn, numLeftMergeOnLeft=leftNumMerge,
numRightTurnsOnRight= rightNumTurn, numRightMergeOnRight=rightNumMerge)
except Exception as e:
raise e
def createCurveByLength(self,
roadId,
length,
isJunction=False,
curvature=StandardCurvature.Medium.value,
n_lanes=1,
lane_offset=3,
laneSides=LaneSides.BOTH,
isLeftTurnLane=False,
isRightTurnLane=False,
isLeftMergeLane=False,
isRightMergeLane=False):
junction = self.getJunctionSelection(isJunction)
pv = ExtendedPlanview()
arc = pyodrx.Arc(curvature, length=length)
pv.add_geometry(arc)
length = pv.getTotalLength()
laneSections = self.laneBuilder.getStandardLanes(
n_lanes,
lane_offset,
laneSides,
roadLength=length,
isLeftTurnLane=isLeftTurnLane,
isRightTurnLane=isRightTurnLane,
isLeftMergeLane=isLeftMergeLane,
isRightMergeLane=isRightMergeLane)
road = ExtendedRoad(roadId,
pv,
laneSections,
road_type=junction,
curveType=StandardCurveTypes.LongArc)
return road
def createS(self,
roadId,
angleBetweenEndpoints,
isJunction=False,
curvature=StandardCurvature.Medium.value,
n_lanes=1,
lane_offset=3,
laneSides=LaneSides.BOTH,
isLeftTurnLane=False,
isRightTurnLane=False,
isLeftMergeLane=False,
isRightMergeLane=False):
"""Here the angleBetweenEndpoints are used for the first road and S mid point, and S mid point and Second road
Args:
roadId ([type]): [description]
angleBetweenEndpoints ([type]): used for the first road and S mid point, and S mid point and Second road. Use negative angles for interesting Ss
isJunction (bool, optional): [description]. Defaults to False.
curvature ([type], optional): [description]. Defaults to StandardCurvature.Medium.value.
Returns:
[type]: [description]
"""
junction = extensions.getJunctionSelection(isJunction)
totalRotation = np.pi - angleBetweenEndpoints
# most of the angleBetweenEndpoints should be assigned to the Arc
arcAngle = totalRotation * 0.9
clothAngle = (totalRotation * 0.1) / 2 # curve more.
arc_curv = curvature
arc_angle = arcAngle
cloth_angle = clothAngle
cloth_start = self.STD_START_CLOTH
pv = ExtendedPlanview()
# adjust sign if angle is negative
if cloth_angle < 0 and arc_curv > 0:
cloth_angle = -cloth_angle
arc_curv = -arc_curv
cloth_start = -cloth_start
arc_angle = -arc_angle
# we are changing the second half of the S to have different arc angle and curvature.
multiplier = np.random.choice(9) / 10
arc_angle2 = arc_angle - arc_angle * multiplier
arc_curv2 = -(arc_curv + arc_curv * multiplier
) # the angle needs to be opposite for the second half.
# create geometries
spiral1 = extensions.ExtendedSpiral(cloth_start,
arc_curv,
angle=cloth_angle)
arc = pyodrx.Arc(arc_curv, angle=arc_angle)
arc2 = pyodrx.Arc(arc_curv2, angle=-arc_angle2)
spiral2 = extensions.ExtendedSpiral(-arc_curv,
cloth_start,
angle=-cloth_angle)
pv.add_geometry(spiral1)
pv.add_geometry(arc)
pv.add_geometry(arc2)
pv.add_geometry(spiral2)
length = pv.getTotalLength()
laneSections = self.laneBuilder.getStandardLanes(
n_lanes,
lane_offset,
laneSides,
roadLength=length,
isLeftTurnLane=isLeftTurnLane,
isRightTurnLane=isRightTurnLane,
isLeftMergeLane=isLeftMergeLane,
isRightMergeLane=isRightMergeLane)
road = ExtendedRoad(roadId,
pv,
laneSections,
road_type=junction,
curveType=StandardCurveTypes.S)
return road
def createS(self,
connectionRoadId,
angleBetweenEndpoints,
isJunction=False,
curvature=StandardCurvature.Medium.value):
"""Here the angleBetweenEndpoints are used for the first road and S mid point, and S mid point and Second road
Args:
connectionRoadId ([type]): [description]
angleBetweenEndpoints ([type]): used for the first road and S mid point, and S mid point and Second road. Use negative angles for interesting Ss
isJunction (bool, optional): [description]. Defaults to False.
curvature ([type], optional): [description]. Defaults to StandardCurvature.Medium.value.
Returns:
[type]: [description]
"""
junction = self.getJunctionSelection(isJunction)
totalRotation = np.pi - angleBetweenEndpoints
# most of the angleBetweenEndpoints should be assigned to the Arc
arcAngle = totalRotation * 0.9
clothAngle = (totalRotation * 0.1) / 2 # curve more.
arc_curv = curvature
arc_angle = arcAngle
cloth_angle = clothAngle
r_id = connectionRoadId
cloth_start = self.STD_START_CLOTH
n_lanes = 1
lane_offset = 3
pv = ExtendedPlanview()
# adjust sign if angle is negative
if cloth_angle < 0 and arc_curv > 0:
cloth_angle = -cloth_angle
arc_curv = -arc_curv
cloth_start = -cloth_start
arc_angle = -arc_angle
# we are changing the second half of the S to have different arc angle and curvature.
multiplier = np.random.choice(9) / 10
arc_angle2 = arc_angle - arc_angle * multiplier
arc_curv2 = -(arc_curv + arc_curv * multiplier
) # the angle needs to be opposite for the second half.
# create geometries
spiral1 = extensions.ExtendedSpiral(cloth_start,
arc_curv,
angle=cloth_angle)
arc = pyodrx.Arc(arc_curv, angle=arc_angle)
arc2 = pyodrx.Arc(arc_curv2, angle=-arc_angle2)
spiral2 = extensions.ExtendedSpiral(-arc_curv,
cloth_start,
angle=-cloth_angle)
pv.add_geometry(spiral1)
pv.add_geometry(arc)
pv.add_geometry(arc2)
pv.add_geometry(spiral2)
# create lanes
lsec = pyodrx.LaneSection(0, pyodrx.standard_lane())
for _ in range(1, n_lanes + 1, 1):
lsec.add_right_lane(pyodrx.standard_lane(lane_offset))
lsec.add_left_lane(pyodrx.standard_lane(lane_offset))
laneSections = extensions.LaneSections()
laneSections.add_lanesection(lsec)
# create road
road = ExtendedRoad(r_id, pv, laneSections, road_type=junction)
road.curveType = StandardCurveTypes.S
return road