def export(self, config):
y = -config.road_width - 0.3 + self._width / 2
rect = schema.rectangle(length=self._length,
width=self._width,
orientation=0,
centerPoint=schema.point(x=self._length / 2,
y=y))
obstacle = schema.obstacle(role="static",
type="parkedVehicle",
shape=schema.shape())
obstacle.shape.rectangle.append(rect)
parking_lane = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
parking_lane.leftBoundary.point.append(
schema.point(x=0, y=-config.road_width))
parking_lane.leftBoundary.point.append(
schema.point(x=self._length, y=-config.road_width))
parking_lane.rightBoundary.point.append(
schema.point(x=0, y=-config.road_width - 0.3))
parking_lane.rightBoundary.point.append(
schema.point(x=self._length, y=-config.road_width - 0.3))
parking_lane.rightBoundary.lineMarking = "solid"
export = super().export(config)
export.objects.append(obstacle)
export.objects.append(parking_lane)
return export
def export(self, config):
export = self._child.export(config)
objects = export.objects
for obj in objects:
if isinstance(obj, schema.lanelet):
for i in range(len(obj.leftBoundary.point)):
x = obj.leftBoundary.point[i].x
y = obj.leftBoundary.point[i].y
transformed = self._transform_point([x, y])
obj.leftBoundary.point[i].x = transformed[0]
obj.leftBoundary.point[i].y = transformed[1]
for i in range(len(obj.rightBoundary.point)):
x = obj.rightBoundary.point[i].x
y = obj.rightBoundary.point[i].y
transformed = self._transform_point([x, y])
obj.rightBoundary.point[i].x = transformed[0]
obj.rightBoundary.point[i].y = transformed[1]
elif isinstance(obj, schema.obstacle):
for rect in obj.shape.rectangle:
x = rect.centerPoint.x
y = rect.centerPoint.y
transformed = self._transform_point([x, y])
rect.orientation -= self._angle
rect.centerPoint = schema.point(x=transformed[0],
y=transformed[1])
elif isinstance(obj, schema.trafficSign):
x = obj.centerPoint.x
y = obj.centerPoint.y
transformed = self._transform_point([x, y])
obj.orientation += self._angle
obj.centerPoint = schema.point(x=transformed[0],
y=transformed[1])
return export
def ego_vehicle():
shape = schema.shape()
shape.rectangle.append(
schema.rectangle(length=0.4,
width=0.4,
orientation=0,
centerPoint=schema.point(x=0, y=0)))
state = schema.state(orientation=schema.floatInterval(exact=0),
time=schema.floatInterval(exact=0))
state.position = schema.point(x=0, y=0)
return schema.egoVehicle(id=0, type="car", shape=shape, initialState=state)
def export(self, config):
zebra = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
zebra.type = "zebraCrossing"
zebra.leftBoundary.point.append(schema.point(x=0,
y=-config.road_width))
zebra.leftBoundary.point.append(schema.point(x=0, y=config.road_width))
zebra.rightBoundary.point.append(
schema.point(x=self._length, y=-config.road_width))
zebra.rightBoundary.point.append(
schema.point(x=self._length, y=config.road_width))
export = super().export(config)
export.objects.append(zebra)
return export
def export(self, config):
parking_lane = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
parking_lane.leftBoundary.point.append(
schema.point(x=0, y=-config.road_width))
parking_lane.leftBoundary.point.append(
schema.point(x=self._length, y=-config.road_width))
parking_lane.rightBoundary.point.append(
schema.point(x=0, y=-config.road_width - 0.3))
parking_lane.rightBoundary.point.append(
schema.point(x=self._length, y=-config.road_width - 0.3))
parking_lane.rightBoundary.lineMarking = "solid"
export = super().export(config)
export.objects.append(parking_lane)
return export
def export(self, config):
points = self.get_points()
lanelet1 = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
lanelet2 = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
if hasattr(self, "_is_start") and self._is_start:
lanelet1.isStart = True
lanelet1.rightBoundary.lineMarking = convert_line_marking(
self._right_line if hasattr(self, "_right_line") else None)
lanelet1.leftBoundary.lineMarking = convert_line_marking(
self._middle_line if hasattr(self, "_middle_line") else None)
lanelet2.rightBoundary.lineMarking = convert_line_marking(
self._left_line if hasattr(self, "_left_line") else None)
for i in range(len(points)):
if i != len(points) - 1:
p1 = np.array(points[i])
p2 = np.array(points[i + 1])
ortho_left = np.array([-(p2[1] - p1[1]), p2[0] - p1[0]])
ortho_left = ortho_left / np.linalg.norm(
ortho_left) * config.road_width
ortho_right = ortho_left * (-1)
else:
p1 = np.array(points[i])
left = p1 + ortho_left
right = p1 + ortho_right
lanelet1.leftBoundary.point.append(
schema.point(x=points[i][0], y=points[i][1]))
lanelet1.rightBoundary.point.append(
schema.point(x=right[0], y=right[1]))
lanelet2.leftBoundary.point.append(
schema.point(x=points[i][0], y=points[i][1]))
lanelet2.rightBoundary.point.append(
schema.point(x=left[0], y=left[1]))
# TODO add last point
# reverse boundary of left lanelet to match driving direction
lanelet2.leftBoundary.point.reverse()
lanelet2.rightBoundary.point.reverse()
return Export([lanelet1, lanelet2], [(lanelet1, lanelet2)])
def export(self, config):
traffic_sign = schema.trafficSign(type=self._traffic_sign,
orientation=math.pi,
centerPoint=schema.point(
x=self._length / 2,
y=-config.road_width - 0.15))
export = super().export(config)
export.objects.append(traffic_sign)
return export
def export(self, config):
rect = schema.rectangle(length=self._length,
width=self._obst_width,
orientation=0,
centerPoint=schema.point(x=self._length / 2,
y=-config.road_width +
self._obst_width / 2))
obstacle = schema.obstacle(role="static",
type="blockedArea",
shape=schema.shape())
obstacle.shape.rectangle.append(rect)
export = super().export(config)
export.objects.append(obstacle)
return export
def export(self, config):
y = self._position * config.road_width
if self._anchor == "left":
y -= self._width / 2
elif self._anchor == "right":
y += self._width / 2
rect = schema.rectangle(length=self._length,
width=self._width,
orientation=0,
centerPoint=schema.point(x=self._length / 2,
y=y))
obstacle = schema.obstacle(role="static",
type="parkedVehicle",
shape=schema.shape())
obstacle.shape.rectangle.append(rect)
export = super().export(config)
export.objects.append(obstacle)
return export
def export(self, config):
southRight = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
southRight.leftBoundary.lineMarking = "dashed"
southRight.rightBoundary.lineMarking = "solid"
southRight.leftBoundary.point.append(schema.point(x=0, y=-self._size))
southRight.leftBoundary.point.append(
schema.point(x=0, y=-config.road_width))
southRight.rightBoundary.point.append(
schema.point(x=config.road_width, y=-self._size))
southRight.rightBoundary.point.append(
schema.point(x=config.road_width, y=-config.road_width))
southLeft = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
southLeft.rightBoundary.lineMarking = "solid"
southLeft.leftBoundary.point.append(
schema.point(x=0, y=-config.road_width))
southLeft.leftBoundary.point.append(schema.point(x=0, y=-self._size))
southLeft.rightBoundary.point.append(
schema.point(x=-config.road_width, y=-config.road_width))
southLeft.rightBoundary.point.append(
schema.point(x=-config.road_width, y=-self._size))
northRight = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
northRight.leftBoundary.lineMarking = "dashed"
northRight.rightBoundary.lineMarking = "solid"
northRight.leftBoundary.point.append(schema.point(x=0, y=self._size))
northRight.leftBoundary.point.append(
schema.point(x=0, y=config.road_width))
northRight.rightBoundary.point.append(
schema.point(x=-config.road_width, y=self._size))
northRight.rightBoundary.point.append(
schema.point(x=-config.road_width, y=config.road_width))
northLeft = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
northLeft.rightBoundary.lineMarking = "solid"
northLeft.leftBoundary.point.append(
schema.point(x=0, y=config.road_width))
northLeft.leftBoundary.point.append(schema.point(x=0, y=self._size))
northLeft.rightBoundary.point.append(
schema.point(x=config.road_width, y=config.road_width))
northLeft.rightBoundary.point.append(
schema.point(x=config.road_width, y=self._size))
eastRight = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
eastRight.rightBoundary.lineMarking = "solid"
eastRight.leftBoundary.point.append(schema.point(x=self._size, y=0))
eastRight.leftBoundary.point.append(
schema.point(x=config.road_width, y=0))
eastRight.rightBoundary.point.append(
schema.point(x=self._size, y=config.road_width))
eastRight.rightBoundary.point.append(
schema.point(x=config.road_width, y=config.road_width))
eastLeft = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
eastLeft.rightBoundary.lineMarking = "solid"
eastLeft.leftBoundary.lineMarking = "dashed"
eastLeft.leftBoundary.point.append(
schema.point(x=config.road_width, y=0))
eastLeft.leftBoundary.point.append(schema.point(x=self._size, y=0))
eastLeft.rightBoundary.point.append(
schema.point(x=config.road_width, y=-config.road_width))
eastLeft.rightBoundary.point.append(
schema.point(x=self._size, y=-config.road_width))
westRight = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
westRight.rightBoundary.lineMarking = "solid"
westRight.leftBoundary.point.append(schema.point(x=-self._size, y=0))
westRight.leftBoundary.point.append(
schema.point(x=-config.road_width, y=0))
westRight.rightBoundary.point.append(
schema.point(x=-self._size, y=-config.road_width))
westRight.rightBoundary.point.append(
schema.point(x=-config.road_width, y=-config.road_width))
westLeft = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
westLeft.leftBoundary.lineMarking = "dashed"
westLeft.rightBoundary.lineMarking = "solid"
westLeft.leftBoundary.point.append(
schema.point(x=-config.road_width, y=0))
westLeft.leftBoundary.point.append(schema.point(x=-self._size, y=0))
westLeft.rightBoundary.point.append(
schema.point(x=-config.road_width, y=config.road_width))
westLeft.rightBoundary.point.append(
schema.point(x=-self._size, y=config.road_width))
if self._rule == "equal":
northRight.stopLine = "dashed"
westRight.stopLine = "dashed"
eastRight.stopLine = "dashed"
southRight.stopLine = "dashed"
elif self._rule == "priority-yield" and self._target_dir == "straight":
westRight.stopLine = "dashed"
eastRight.stopLine = "dashed"
elif self._rule == "priority-stop" and self._target_dir == "straight":
westRight.stopLine = "solid"
eastRight.stopLine = "solid"
elif self._rule == "yield":
northRight.stopLine = "dashed"
southRight.stopLine = "dashed"
elif self._rule == "stop":
northRight.stopLine = "solid"
southRight.stopLine = "solid"
result = [
southRight, southLeft, northLeft, northRight, eastLeft, eastRight,
westLeft, westRight
]
pairs = [(southRight, southLeft)]
if self._target_dir == "left":
right_lanelet = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
right_lanelet.rightBoundary.lineMarking = "dashed"
right_lanelet.leftBoundary.lineMarking = "dashed"
for angle in np.arange(0, math.pi / 2, math.pi / 20):
right_lanelet.rightBoundary.point.append(
schema.point(x=-config.road_width +
math.cos(angle) * config.road_width * 2,
y=-config.road_width +
math.sin(angle) * config.road_width * 2))
right_lanelet.leftBoundary.point.append(
schema.point(x=-config.road_width +
math.cos(angle) * config.road_width,
y=-config.road_width +
math.sin(angle) * config.road_width))
left_lanelet = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
for angle in np.arange(math.pi / 2, 0, -math.pi / 20):
left_lanelet.leftBoundary.point.append(
schema.point(x=-config.road_width +
math.cos(angle) * config.road_width,
y=-config.road_width +
math.sin(angle) * config.road_width))
left_lanelet.rightBoundary.point.append(
schema.point(x=-config.road_width, y=-config.road_width))
result.append(right_lanelet)
result.append(left_lanelet)
pairs.append((right_lanelet, left_lanelet))
pairs.append((westLeft, westRight))
result.append(
schema.trafficSign(type="stvo-209-10",
orientation=math.pi * 1.5,
centerPoint=schema.point(
x=config.road_width + 0.1,
y=-config.road_width - 0.25)))
elif self._target_dir == "right":
right_lanelet = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
right_lanelet.leftBoundary.lineMarking = "dashed"
for angle in np.arange(math.pi, math.pi / 2, -math.pi / 20):
right_lanelet.rightBoundary.point.append(
schema.point(x=config.road_width, y=-config.road_width))
right_lanelet.leftBoundary.point.append(
schema.point(x=config.road_width +
math.cos(angle) * config.road_width,
y=-config.road_width +
math.sin(angle) * config.road_width))
left_lanelet = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
left_lanelet.rightBoundary.lineMarking = "dashed"
for angle in np.arange(math.pi / 2, math.pi, math.pi / 20):
left_lanelet.leftBoundary.point.append(
schema.point(x=config.road_width +
math.cos(angle) * config.road_width,
y=-config.road_width +
math.sin(angle) * config.road_width))
left_lanelet.rightBoundary.point.append(
schema.point(x=config.road_width +
math.cos(angle) * config.road_width * 2,
y=-config.road_width +
math.sin(angle) * config.road_width * 2))
result.append(right_lanelet)
result.append(left_lanelet)
pairs.append((right_lanelet, left_lanelet))
pairs.append((eastLeft, eastRight))
result.append(
schema.trafficSign(type="stvo-209-20",
orientation=math.pi * 1.5,
centerPoint=schema.point(
x=config.road_width + 0.1,
y=-config.road_width - 0.25)))
elif self._target_dir == "straight":
right_lanelet = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
right_lanelet.rightBoundary.point.append(
schema.point(x=config.road_width, y=-config.road_width))
right_lanelet.rightBoundary.point.append(
schema.point(x=config.road_width, y=config.road_width))
right_lanelet.leftBoundary.point.append(
schema.point(x=0, y=-config.road_width))
right_lanelet.leftBoundary.point.append(
schema.point(x=0, y=config.road_width))
left_lanelet = schema.lanelet(leftBoundary=schema.boundary(),
rightBoundary=schema.boundary())
left_lanelet.rightBoundary.point.append(
schema.point(x=-config.road_width, y=config.road_width))
left_lanelet.rightBoundary.point.append(
schema.point(x=-config.road_width, y=-config.road_width))
left_lanelet.leftBoundary.point.append(
schema.point(x=0, y=config.road_width))
left_lanelet.leftBoundary.point.append(
schema.point(x=0, y=-config.road_width))
result.append(right_lanelet)
result.append(left_lanelet)
pairs.append((right_lanelet, left_lanelet))
pairs.append((northLeft, northRight))
type_map = {
"priority-yield": "stvo-306",
"priority-stop": "stvo-306",
"yield": "stvo-205",
"stop": "stvo-206"
}
if self._rule in type_map:
result.append(
schema.trafficSign(type=type_map[self._rule],
orientation=math.pi * 1.5,
centerPoint=schema.point(
x=config.road_width + 0.1,
y=-config.road_width - 0.5)))
return Export(result, pairs)