def _lane_msg_from_lines(left: Line, middle: Line, right: Line) -> LaneMsg:
"""Create a lane message from three lines."""
msg = LaneMsg()
msg.middle_line = middle.to_geometry_msgs()
msg.left_line = left.to_geometry_msgs()
msg.right_line = right.to_geometry_msgs()
return msg
def parking_msgs(
right_spots: List[Polygon],
left_spots: List[Polygon],
right_border: Line,
left_border: Line,
id: int,
) -> groundtruth_srvs.ParkingSrvResponse:
response = groundtruth_srvs.ParkingSrvResponse()
right_msg = groundtruth_msgs.Parking()
right_msg.borders = [
groundtruth_msgs.Line(right_border.to_geometry_msgs())
]
right_msg.spots = list(
groundtruth_msgs.LabeledPolygon(spot.to_geometry_msg(),
groundtruth_msgs.Parking.SPOT_FREE)
for spot in right_spots)
response.right_msg = right_msg
left_msg = groundtruth_msgs.Parking()
left_msg.borders = [groundtruth_msgs.Line(left_border.to_geometry_msgs())]
left_msg.spots = list(
groundtruth_msgs.LabeledPolygon(spot.to_geometry_msg(),
groundtruth_msgs.Parking.SPOT_FREE)
for spot in left_spots)
response.left_msg = left_msg
return response
def intersection_proxy(id):
lane = groundtruth_msgs.Lane()
if id == 2:
lane.middle_line = Line([[19, 0], [20, 0]]).to_geometry_msgs()
if id == 3:
lane.middle_line = Line([[29, 0], [30, 0]]).to_geometry_msgs()
return fake_msgs.intersection_msg(rule=self._rules[id], south=lane)
def setUp(self):
n_section = 1
n_points = 100
lane_width = 0.4
# Create points
lines = fake_msgs.create_points(
section_count=n_section,
point_count=n_points,
offset_right=lane_width,
direction=0,
deviation=0,
)
self.right_border = Line([[0, -lane_width], [1, -2 * lane_width],
[2, -2 * lane_width], [3, -lane_width]])
self.left_border = Line([[0, lane_width], [1, 2 * lane_width],
[2, 2 * lane_width], [3, lane_width]])
# Fake section and lane msg proxies / usually in the groundtruth package
section_msg_proxy = functools.partial(fake_msgs.section_msgs,
section_count=n_section)
lane_msg_proxy = functools.partial(fake_msgs.lane_msgs, lines)
parking_msg_proxy = functools.partial(fake_msgs.parking_msgs, [], [],
self.right_border,
self.left_border)
self.speaker = AreaSpeaker(
section_proxy=section_msg_proxy,
lane_proxy=lane_msg_proxy,
parking_proxy=parking_msg_proxy,
min_wheel_count=3,
area_buffer=0.001,
)
def get_road_lines(self, section_id: int) -> LineTuple:
"""Request and return the road lines of a single section.
Args:
section_id (int): id of the section
Returns:
LineTuple of the section as a named tuple.
"""
msg = self.get_lanes(section_id).lane_msg
return LineTuple(Line(msg.left_line), Line(msg.middle_line), Line(msg.right_line))
def create_intersection(id: int,
rule: int = IntersectionSrvResponse.EQUAL,
lane_width: float = 0.4):
"""Mock intersection to be used for testing.
Args:
id: ID of the section.
rule: Priority rule of intersection.
lane_width: width of right and left lane.
"""
tf = Transform([2 * id, 0], 0)
middle_line = tf * Line([[0, 0], [2, 0]])
left_line = middle_line.parallel_offset(lane_width, side="left")
right_line = middle_line.parallel_offset(lane_width, side="right")
south_middle = tf * Line([[0, 0], [0.75, 0]])
north_middle = tf * Line([[1.25, 0], [2, 0]])
return section_mocks.mock_intersection(
id=1,
type_=road_section_type.INTERSECTION,
left_line=left_line,
middle_line=middle_line,
right_line=right_line,
turn=IntersectionSrvResponse.STRAIGHT,
rule=rule,
south=LineTuple(
south_middle.parallel_offset(lane_width, side="left"),
south_middle,
south_middle.parallel_offset(lane_width, side="right"),
),
west=LineTuple(
tf * Line([[1 - lane_width, lane_width],
[1 - lane_width, 3 * lane_width]]),
tf * Line([[1, lane_width], [1, 3 * lane_width]]),
tf * Line([[1 + lane_width, lane_width],
[1 + lane_width, 3 * lane_width]]),
),
east=LineTuple(
tf * Line([[1 - lane_width, -lane_width],
[1 - lane_width, -3 * lane_width]]),
tf * Line([[1, -lane_width], [1, -3 * lane_width]]),
tf * Line([[1 + lane_width, -lane_width],
[1 + lane_width, -3 * lane_width]]),
),
north=LineTuple(
north_middle.parallel_offset(lane_width, side="left"),
north_middle,
north_middle.parallel_offset(lane_width, side="right"),
),
# surface_markings=[
# (
# tf * Polygon([[0.3, -0.1], [0.3, -0.3], [0.6, -0.3], [0.6, -0.1]]),
# LabeledPolygonMsg.RIGHT_TURN_MARKING,
# )
# ],
)
def _get_stop_line(line1: Line, line2: Line, kind) -> SurfaceMarkingRect:
"""Return a line perpendicular to both provided (assumed parallel) lines.
The returned line will be at the first point where both lines are parallel to each other
plus 2cm offset.
"""
beginning_line1 = line1.interpolate(0.02)
beginning_line2 = line2.interpolate(0.02)
# Test which line to draw the stop line at
if beginning_line1.distance(line2) < beginning_line2.distance(line1):
# End of line 1 is the starting point of the stop line
p1 = beginning_line1
p2 = line2.interpolate(line2.project(beginning_line1))
else:
# End of line 2 is the starting point
p1 = line1.interpolate(line1.project(beginning_line2))
p2 = beginning_line2
line = Line([p1, p2])
width = line.length
center = 0.5 * (Vector(line.coords[0]) + Vector(line.coords[1]))
angle = line1.interpolate_direction(arc_length=0).argument
return SurfaceMarkingRect(kind,
*center.xy,
angle=angle,
width=width,
normalize_x=False,
depth=0.04)
def lines(self) -> List[MarkedLine]:
"""List[MarkedLine]: Borderlines for spot if spot is on the left.
Marking type is always solid."""
lines = []
if self._side == ParkingLot.LEFT_SIDE or self.kind == self.BLOCKED:
spot_points = self.frame.get_points()
left_border = Line(spot_points[:2])
right_border = Line(spot_points[2:4])
lines.append(
MarkedLine.from_line(left_border,
RoadSection.SOLID_LINE_MARKING))
lines.append(
MarkedLine.from_line(right_border,
RoadSection.SOLID_LINE_MARKING))
return lines
def set_transform(self, line: Line):
"""Calculate the correct transform to this element.
Depending on :attr:`self.normalize_x` the positional behavior is different.
If :attr:`self.normalize_x` is True, the element is aligned along the provided line.
Example:
>>> from simulation.utils.geometry import Line, Point, Transform
>>> from simulation.utils.road.sections.road_element import RoadElementRect
>>> line = Line([Point(0, 0), Point(0, 10)]) # y axis
>>> normalized_el = RoadElementRect(center=Point(1, 1)) # normalize_x is True by default
>>> normalized_el.set_transform(line)
>>> normalized_el.transform
Transform(translation=(0.0, 1.0, 0.0),rotation=90.0 degrees)
>>> normalized_el._center
Point(1.0, 1.0, 0.0)
>>> normalized_el.center
Point(-1.0, 1.0, 0.0)
>>> unnormalized_el = RoadElementRect(center=Point(1,0), normalize_x=False)
... # normalize_x is True by default
>>> unnormalized_el.set_transform(line)
>>> unnormalized_el.transform
Transform(translation=(0.0, 0.0, 0.0),rotation=90.0 degrees)
>>> normalized_el._center
Point(1.0, 1.0, 0.0)
>>> normalized_el.center
Point(-1.0, 1.0, 0.0)
"""
pose = line.interpolate_pose(
arc_length=self._center.x if self.normalize_x else 0)
self.transform = Transform(pose, pose.get_angle())
def middle_line(self) -> Line:
"""Complete middle line."""
# Get the middle line of each seaction
middle_line_pieces = (self.get_road_lines(sec.id).middle
for sec in self.sections)
# Sum it up (and start with empty line)
return sum(middle_line_pieces, Line())
def right_line(self) -> Line:
"""Complete right line."""
# Get the right line of each seaction
right_line_pieces = (self.get_road_lines(sec.id).right
for sec in self.sections)
# Sum it up (and start with empty line)
return sum(right_line_pieces, Line())
def create_points(
start_point=Point(0, 0),
offset_right=1,
point_dist=1,
section_count=10,
point_count=100,
direction=0,
deviation=math.pi / 8,
) -> List[LineTuple]:
"""Create a line of points and split it up into multiple sections."""
# Points that are biased in one direction
points = [start_point] + list(
Vector(r=point_dist,
phi=direction + (random.random() - 0.5) * 2 * deviation)
for _ in range(point_count))
# Prefix sum over the points to get the middle line points
middle_points = list(itertools.accumulate(points))
# Divide the middle points into multiple sections
def divide_chunks():
for i in range(0, point_count, int(point_count / section_count)):
yield middle_points[i:i + int(point_count / section_count) +
1 # noqa: 203
] # Also include first point of next section
middle_lines = (Line(ps) for ps in divide_chunks())
return list(
LineTuple(
middle_line.parallel_offset(offset_right, side="left"),
middle_line,
middle_line.parallel_offset(offset_right, side="right"),
) for middle_line in middle_lines)
def ls_circle(self) -> Line:
if self.turn == Intersection.LEFT:
points_ls = []
for theta in arange_with_end(0, 0.5 * math.pi + self._alpha,
math.pi / 20):
points_ls.append(
Point(self.z - self.x + self.ls - self.ls.rotated(theta)))
return self.transform * Line(points_ls)
def test_straight_road(self):
TF = Transform([1, 1], math.pi / 2)
LENGTH = 4
MIDDLE_LINE = Line([[1, 1], [1, 5]])
sr = StraightRoad(length=LENGTH, transform=TF)
self.assertEqual(sr.__class__.TYPE, road_section_type.STRAIGHT_ROAD)
self.assertEqual(sr.middle_line, MIDDLE_LINE)
def rs_circle(self) -> Line:
if self.turn == Intersection.RIGHT:
points_rs = []
for theta in arange_with_end(0, -math.pi / 2 + self._alpha,
-math.pi / 20):
points_rs.append(
Point(self.z - self.x + self.rs - self.rs.rotated(theta)))
return self.transform * Line(points_rs)
def middle_line_l(self) -> Line:
"""Line: Middle line on the left side of the traffic island."""
return (self.transform * Line([
self.middle_start,
*self.bezier_points_mid_l_start,
self.middle_l_zebra_start,
self.middle_l_zebra_end,
*self.bezier_points_mid_l_end,
self.middle_end,
]).simplify())
def test_road_section(self):
MIDDLE_LINE = Line([[0, 0], [1, 0]])
DummyRoadSection.MIDDLE_LINE = MIDDLE_LINE
LEFT_LINE = Line([[0, Config.road_width], [1, Config.road_width]])
RIGHT_LINE = Line([[0, -Config.road_width],
[1 + Config.road_width, -Config.road_width]])
ENDING = (Pose([1, 0], 0), 0)
BEGINNING = (Pose([0, 0], math.pi), 0)
rs = DummyRoadSection()
self.assertIsNotNone(rs.transform)
self.assert_lines_approx_equal(rs.middle_line, MIDDLE_LINE)
self.assert_lines_approx_equal(rs.right_line, RIGHT_LINE)
self.assert_lines_approx_equal(rs.left_line, LEFT_LINE)
self.assertTupleEqual(rs.get_beginning(), BEGINNING)
self.assertTupleEqual(rs.get_ending(), ENDING)
self.assertTrue(rs.get_bounding_box().contains(LEFT_LINE))
self.assertTrue(rs.get_bounding_box().contains(MIDDLE_LINE))
self.assertTrue(rs.get_bounding_box().contains(RIGHT_LINE))
def middle_line_r(self) -> Line:
"""Line: Middle line on the right side of the traffic island."""
return (self.transform * Line([
self.middle_start,
*self.bezier_points_mid_r_start,
self.middle_r_zebra_start,
self.middle_r_zebra_end,
*self.bezier_points_mid_r_end,
self.middle_end,
]).simplify())
"""The simplification is necessary to be able to draw the blocked areas of the island.
def middle_line(self) -> Line:
"""Line: Middle line of the intersection."""
if self.turn == Intersection.LEFT:
return self.middle_line_south + self.ls_circle + self.middle_line_west
elif self.turn == Intersection.RIGHT:
return self.middle_line_south + self.rs_circle + self.middle_line_east
else:
straight_m_l = Line([
self.middle_line_south.get_points()[-1],
self.middle_line_north.get_points()[0],
])
return self.middle_line_south + straight_m_l + self.middle_line_north
def draw_start_lane(ctx, frame: Polygon):
"""Draw the checkerboard pattern to mark the beginning of a parking area in the given
frame.
Args:
frame: Frame of the start lane.
**Points of the frame must be given in the right order!**
first point : start on left line
second point: end on left line
third point : end on right line
fourth point: start on right line
"""
TILE_LENGTH = 0.02
points = frame.get_points()
left = Line([points[0], points[1]])
right = Line([points[3], points[2]])
for i in range(3):
utils.draw_line(
ctx,
MarkedLine(
[
left.interpolate(TILE_LENGTH * (i + 0.5)),
right.interpolate(TILE_LENGTH * (i + 0.5)),
],
style=RoadSection.DASHED_LINE_MARKING,
prev_length=(i % 2 + 0.5) * TILE_LENGTH,
),
dash_length=TILE_LENGTH,
)
def draw_zebra_crossing(ctx,
frame: Polygon,
stripe_width: float = 0.04,
offset: float = 0.02):
"""Draw a zebra crossing in the given frame.
Args:
frame: Frame of the zebra crossing.
**Points of the frame must be given in the right order!**
first point : start on left line
second point: end on left line
third point : end on right line
fourth point: start on right line
"""
points = frame.get_points()
left = Line([points[0], points[3]])
right = Line([points[1], points[2]])
flag = True
min_length = min(left.length, right.length)
x = offset
while x < min_length:
l, r = left.interpolate(x), right.interpolate(x)
if flag:
ctx.move_to(l.x, l.y)
ctx.line_to(r.x, r.y)
flag = False
else:
ctx.line_to(r.x, r.y)
ctx.line_to(l.x, l.y)
ctx.close_path()
ctx.fill()
flag = True
x += stripe_width
def middle_line(self) -> Line:
points = []
t = 0
while t <= 1:
points.append(
Point(*_compute_cubic_bezier(t, self._p0, self._p1, self._p2,
self._p3)))
t += 0.01
t = 1
points.append(
Point(*_compute_cubic_bezier(t, self._p0, self._p1, self._p2,
self._p3)))
return self.transform * Line(points)
def draw_blocked_stripes(ctx, v: Vector, start: Point, line: Line,
points: List[Point], angle: float, gap: float):
"""Draw white stripes onto the ground.
White stripes are e.g. used by to signal areas on the ground
where the car is not allowed to drive.
Args:
v: Vector along the line where the stripes start points are located.
start: Start point on the line where the stripes start points are located.
line: End points of the stripes are on this line.
points: List of points of the polygon frame.
angle: Angle of the stripes.
gap: Gap between the stripes.
"""
ctx.save()
v = gap / abs(v) * v
stripe = v.rotated(math.pi + angle)
# vetcor in direction of stripes
stripe = 2.5 * Config.road_width / abs(stripe) * stripe
for point in points:
ctx.line_to(point.x, point.y)
ctx.stroke_preserve()
ctx.clip()
ctx.set_line_width(0.02)
while True:
start += v
p = Point(start + stripe)
end = line.intersection(Line([start, p]))
if end.is_empty:
break
ctx.move_to(end.x, end.y)
ctx.line_to(start.x, start.y)
ctx.stroke()
ctx.restore()
def middle_line(self) -> Line:
"""Line: Line in the middle of the road."""
# Get all sections
sections: List[int] = self.section_proxy().sections
assert len(sections) > 0, (
"There must be atleast one road section. "
"(The groundtruth node might not be working correctly.)"
)
# Concatenate the middle line of all sections
return sum(
(Line(self.lane_proxy(sec.id).lane_msg.middle_line) for sec in sections), Line()
)
def middle_line(self) -> Line:
points = []
t = 0
while t <= 1:
c0 = (1 - t) * self._p0 + t * self._p1
c1 = (1 - t) * self._p1 + t * self._p2
x = (1 - t) * c0 + t * c1
points.append(Point(*x))
t += 0.01
t = 1
c0 = (1 - t) * self._p0 + t * self._p1
c1 = (1 - t) * self._p1 + t * self._p2
x = (1 - t) * c0 + t * c1
points.append(Point(*x))
return self.transform * Line(points)
def test_obstacles(self):
MIDDLE_LINE = Line([[0, 0], [0, 2]])
DummyRoadSection.MIDDLE_LINE = MIDDLE_LINE
# Assume the obstacle class itself works!
# Simply test if the transforms etc. are set correctly.
obstacle = StaticObstacle(center=Point(1.5, -0.2), width=0.2, depth=1)
rs = DummyRoadSection(obstacles=[obstacle])
returned_obs = rs.obstacles[0]
self.assertEqual(returned_obs.transform,
Transform([0, 1.5], math.pi / 2))
self.assertEqual(returned_obs.center, Point(0.2, 1.5))
self.assertEqual(returned_obs.frame,
Polygon([[0.1, 1], [0.1, 2], [0.3, 2], [0.3, 1]]))
def border(self) -> Line:
"""Line: Outside border of the parking lot."""
if self.depth is None:
return Line()
border_points = [
Point(0, self.depth / math.tan(self.opening_angle)),
Point(
self.depth,
0,
),
Point(
self.depth,
-self.length,
),
Point(
0,
-self.depth / math.tan(self.opening_angle) - self.length,
),
]
border = Line(
reversed(border_points) if self._side ==
ParkingLot.RIGHT_SIDE else border_points)
return self.transform * border
def middle_line(self) -> Line:
RADIAN_STEP = math.pi / 360
points = []
current_angle = 0
radius = (-1 if self.__class__.TYPE
== road_section_type.RIGHT_CIRCULAR_ARC else 1) * self.radius
while current_angle <= self.angle + RADIAN_STEP / 2:
points.append(
Point(
math.cos(current_angle - math.pi / 2) * abs(radius),
radius + math.sin(current_angle - math.pi / 2) * radius,
))
current_angle += RADIAN_STEP
return self.transform * Line(points)
def driving_line(self) -> Line:
"""Line: Line in the middle of the right lane (where car drives)."""
# Get all sections
sections: List[int] = self.section_proxy().sections
assert len(sections) > 0, (
"There must be atleast one road section. "
"(The groundtruth node might not be working correctly.)"
)
# Concatenate the middle line of all sections
middle_line = sum(
(Line(self.lane_proxy(sec.id).lane_msg.middle_line) for sec in sections), Line()
)
# Shift to the right to get the middle of the right lane
driving_line = middle_line.parallel_offset(self.param.road_width / 2, "right")
return driving_line
def fake_car_state(self, path: Line, arc_length: float, speed: float):
"""Publish a CarState message depending on situation.
Args:
path: The car drives on this path.
arc_length: Current arc length of the car on the path.
speed: Speed the car drives with.
"""
pose = path.interpolate_pose(arc_length=arc_length)
msg = CarStateMsg()
msg.pose = pose.to_geometry_msg()
msg.frame = (
Transform(pose.to_geometry_msg())
* Polygon([[-0.1, -0.1], [-0.1, 0.1], [0.1, 0.1], [0.1, -0.1]])
).to_geometry_msg()
msg.twist = Twist()
msg.twist.linear = Vector(speed, 0, 0).to_geometry_msg()
self.car_state_publisher.publish(msg)